Examples with PointTrack boofcv.abst.feature.tracker.PointTrack used on opensource projects

Example 26 with PointTrack

use of boofcv.abst.feature.tracker.PointTrack in project BoofCV by lessthanoptimal.

the class VisOdomMonoPlaneInfinity method addNewTracks.

/**
 * Requests that new tracks are spawned, determines if they are on the plane or not, and computes other required
 * data structures.
 */
private void addNewTracks() {
    tracker.spawnTracks();
    List<PointTrack> spawned = tracker.getNewTracks(null);
    // estimate 3D coordinate using stereo vision
    for (PointTrack t : spawned) {
        // System.out.println("track spawn "+t.x+" "+t.y);
        VoTrack p = t.getCookie();
        if (p == null) {
            t.cookie = p = new VoTrack();
        }
        // compute normalized image coordinate
        pixelToNorm.compute(t.x, t.y, n);
        // See if the point ever intersects the ground plane or not
        if (planeProjection.normalToPlane(n.x, n.y, p.ground)) {
            // the line above computes the 2D plane position of the point
            p.onPlane = true;
        } else {
            // Handle points at infinity which are off plane.
            // rotate observation pointing vector into plane reference frame
            pointing.set(n.x, n.y, 1);
            GeometryMath_F64.mult(cameraToPlane.getR(), pointing, pointing);
            pointing.normalize();
            // save value of y-axis in pointing vector
            double normXZ = Math.sqrt(pointing.x * pointing.x + pointing.z * pointing.z);
            p.pointingY = pointing.y / normXZ;
            // save the angle as a vector
            p.ground.x = pointing.z;
            p.ground.y = -pointing.x;
            // normalize to make later calculations easier
            p.ground.x /= normXZ;
            p.ground.y /= normXZ;
            p.onPlane = false;
        }
        p.lastInlier = tick;
    }
}

Example 27 with PointTrack

use of boofcv.abst.feature.tracker.PointTrack in project BoofCV by lessthanoptimal.

the class VisOdomPixelDepthPnP method estimateMotion.

/**
 * Estimates motion from the set of tracks and their 3D location
 *
 * @return true if successful.
 */
private boolean estimateMotion() {
    List<PointTrack> active = tracker.getActiveTracks(null);
    List<Point2D3D> obs = new ArrayList<>();
    for (PointTrack t : active) {
        Point2D3D p = t.getCookie();
        pixelToNorm.compute(t.x, t.y, p.observation);
        obs.add(p);
    }
    // estimate the motion up to a scale factor in translation
    if (!motionEstimator.process(obs))
        return false;
    if (doublePass) {
        if (!performSecondPass(active, obs))
            return false;
    }
    tracker.finishTracking();
    Se3_F64 keyToCurr;
    if (refine != null) {
        keyToCurr = new Se3_F64();
        refine.fitModel(motionEstimator.getMatchSet(), motionEstimator.getModelParameters(), keyToCurr);
    } else {
        keyToCurr = motionEstimator.getModelParameters();
    }
    keyToCurr.invert(currToKey);
    // mark tracks as being inliers and add to inlier list
    int N = motionEstimator.getMatchSet().size();
    for (int i = 0; i < N; i++) {
        int index = motionEstimator.getInputIndex(i);
        Point2D3DTrack t = active.get(index).getCookie();
        t.lastInlier = tick;
        inlierTracks.add(t);
    }
    return true;
}

Example 28 with PointTrack

use of boofcv.abst.feature.tracker.PointTrack in project BoofCV by lessthanoptimal.

the class VisOdomPixelDepthPnP method addNewTracks.

/**
 * Detects new features and computes their 3D coordinates
 */
private void addNewTracks() {
    // System.out.println("----------- Adding new tracks ---------------");
    tracker.spawnTracks();
    List<PointTrack> spawned = tracker.getNewTracks(null);
    // estimate 3D coordinate using stereo vision
    for (PointTrack t : spawned) {
        Point2D3DTrack p = t.getCookie();
        if (p == null) {
            t.cookie = p = new Point2D3DTrack();
        }
        // discard point if it can't localized
        if (!pixelTo3D.process(t.x, t.y) || pixelTo3D.getW() == 0) {
            tracker.dropTrack(t);
        } else {
            Point3D_F64 X = p.getLocation();
            double w = pixelTo3D.getW();
            X.set(pixelTo3D.getX() / w, pixelTo3D.getY() / w, pixelTo3D.getZ() / w);
            // translate the point into the key frame
            // SePointOps_F64.transform(currToKey,X,X);
            // not needed since the current frame was just set to be the key frame
            p.lastInlier = tick;
            pixelToNorm.compute(t.x, t.y, p.observation);
        }
    }
}

Example 29 with PointTrack

use of boofcv.abst.feature.tracker.PointTrack in project BoofCV by lessthanoptimal.

the class VisOdomPixelDepthPnP method dropUnusedTracks.

/**
 * Removes tracks which have not been included in the inlier set recently
 *
 * @return Number of dropped tracks
 */
private int dropUnusedTracks() {
    List<PointTrack> all = tracker.getAllTracks(null);
    int num = 0;
    for (PointTrack t : all) {
        Point2D3DTrack p = t.getCookie();
        if (tick - p.lastInlier > thresholdRetire) {
            tracker.dropTrack(t);
            num++;
        }
    }
    return num;
}

Example 30 with PointTrack

use of boofcv.abst.feature.tracker.PointTrack in project BoofCV by lessthanoptimal.

the class WrapImageMotionPtkSmartRespawn method checkInitialize.

private void checkInitialize() {
    if (!inliersMarked) {
        inliersMarked = true;
        List<PointTrack> active = alg.getMotion().getTracker().getActiveTracks(null);
        allTracks.clear();
        long tick = alg.getMotion().getTotalFramesProcessed();
        inliers.resize(active.size());
        for (int i = 0; i < active.size(); i++) {
            PointTrack t = active.get(i);
            AssociatedPairTrack info = t.getCookie();
            allTracks.add(t);
            // if it was used in the previous update then it is in the inlier set
            inliers.data[i] = info.lastUsed == tick;
        }
    }
}