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

Example 26 with PointTrack

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

the class VisOdomMonoDepthPnP method addObservationsOfInliersToScene.

private void addObservationsOfInliersToScene(List<PointTrack> active) {
    // mark tracks as being inliers and add to inlier list
    int N = motionEstimator.getMatchSet().size();
    long frameID = getFrameID();
    for (int i = 0; i < N; i++) {
        int index = motionEstimator.getInputIndex(i);
        PointTrack pt = active.get(index);
        Track bt = pt.getCookie();
        bt.lastUsed = frameID;
        bt.hasBeenInlier = true;
        bundleViso.addObservation(frameCurrent, bt, pt.pixel.x, pt.pixel.y);
        inlierTracks.add(bt);
    }
}

Example 27 with PointTrack

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

the class VisOdomMonoPlaneInfinity method sortTracksForEstimation.

/**
 * Splits the set of active tracks into on plane and infinity sets. For each set also perform specific sanity
 * checks to make sure basic constraints are still being meet. If not then the track will not be considered for
 * motion estimation.
 */
private void sortTracksForEstimation() {
    // reset data structures
    planeSamples.reset();
    farAngles.reset();
    tracksOnPlane.clear();
    tracksFar.clear();
    // list of active tracks
    List<PointTrack> active = tracker.getActiveTracks(null);
    for (PointTrack t : active) {
        // lint:forbidden ignore_line
        VoTrack p = t.getCookie();
        // compute normalized image coordinate
        pixelToNorm.compute(t.pixel.x, t.pixel.y, n);
        // rotate pointing vector into plane reference frame
        pointing.setTo(n.x, n.y, 1);
        GeometryMath_F64.mult(cameraToPlane.getR(), pointing, pointing);
        pointing.normalize();
        if (p.onPlane) {
            // see if it still intersects the plane
            if (pointing.y > 0) {
                // create data structure for robust motion estimation
                PlanePtPixel ppp = planeSamples.grow();
                ppp.normalizedCurr.setTo(n);
                ppp.planeKey.setTo(p.ground);
                tracksOnPlane.add(t);
            }
        } else {
            // if the point is not on the plane visually and (optionally) if it passes a strict y-axis rotation
            // test, consider using the point for estimating rotation.
            boolean allGood = pointing.y < 0;
            if (strictFar) {
                allGood = isRotationFromAxisY(t, pointing);
            }
            // is it still above the ground plane and only has motion consistent with rotation on ground plane axis
            if (allGood) {
                computeAngleOfRotation(t, pointing);
                tracksFar.add(t);
            }
        }
    }
}

Example 28 with PointTrack

use of boofcv.abst.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) {
        // lint:forbidden ignore_line
        // 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.pixel.x, t.pixel.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.setTo(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 = tracker.getFrameID();
    }
}

Example 29 with PointTrack

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

the class VisOdomMonoPlaneInfinity 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;
    long frameID = tracker.getFrameID();
    for (PointTrack t : all) {
        // lint:forbidden ignore_line
        VoTrack p = t.getCookie();
        if (frameID - p.lastInlier > thresholdRetire) {
            tracker.dropTrack(t);
            num++;
        }
    }
    return num;
}

Example 30 with PointTrack

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

the class VisOdomMonoPlaneInfinity method estimateFar.

/**
 * Estimates only rotation using points at infinity. A robust estimation algorithm is used which finds an angle
 * which maximizes the inlier set, like RANSAC does. Unlike RANSAC this will produce an optimal result.
 */
private void estimateFar() {
    // do nothing if there are objects at infinity
    farInlierCount = 0;
    if (farAngles.size == 0)
        return;
    farAnglesCopy.reset();
    farAnglesCopy.addAll(farAngles);
    // find angle which maximizes inlier set
    farAngle = maximizeCountInSpread(farAnglesCopy.data, farAngles.size, 2 * thresholdFarAngleError);
    // mark and count inliers
    for (int i = 0; i < tracksFar.size(); i++) {
        PointTrack t = tracksFar.get(i);
        VoTrack p = t.getCookie();
        if (UtilAngle.dist(farAngles.get(i), farAngle) <= thresholdFarAngleError) {
            p.lastInlier = tracker.getFrameID();
            farInlierCount++;
        }
    }
}