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

Example 41 with PointTrack

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

the class VisOdomDualTrackPnP method addNewTracks.

/**
 * Spawns tracks in each image and associates features together.
 */
private void addNewTracks() {
    CameraModel leftCM = cameraModels.get(CAMERA_LEFT);
    CameraModel rightCM = cameraModels.get(CAMERA_RIGHT);
    final long frameID = getFrameID();
    trackerLeft.spawnTracks();
    trackerRight.spawnTracks();
    List<PointTrack> spawnedLeft = trackerLeft.getNewTracks(null);
    List<PointTrack> spawnedRight = trackerRight.getNewTracks(null);
    // get a list of new tracks and their descriptions
    describeSpawnedTracks(inputLeft, spawnedLeft, pointsLeft, descLeft);
    describeSpawnedTracks(inputRight, spawnedRight, pointsRight, descRight);
    // associate using L2R
    assocL2R.setSource(pointsLeft, descLeft);
    assocL2R.setDestination(pointsRight, descRight);
    assocL2R.associate();
    FastAccess<AssociatedIndex> matches = assocL2R.getMatches();
    int total = 0;
    for (int i = 0; i < matches.size; i++) {
        AssociatedIndex m = matches.get(i);
        PointTrack trackL = spawnedLeft.get(m.src);
        PointTrack trackR = spawnedRight.get(m.dst);
        TrackInfo bt = bundleViso.tracks.grow();
        // convert pixel observations into normalized image coordinates
        leftCM.pixelToNorm.compute(trackL.pixel.x, trackL.pixel.y, normLeft);
        rightCM.pixelToNorm.compute(trackR.pixel.x, trackR.pixel.y, normRight);
        // triangulate 3D coordinate in the current camera frame
        if (triangulate2.triangulate(normLeft, normRight, left_to_right, cameraP3)) {
            // put the track into the world coordinate system
            SePointOps_F64.transform(currentLeft.frame_to_world, cameraP3, cameraP3);
            bt.worldLoc.setTo(cameraP3.x, cameraP3.y, cameraP3.z, 1.0);
            // Finalize the track data structure
            bt.id = trackL.featureId;
            bt.visualTrack = trackL;
            bt.visualRight = trackR;
            bt.lastStereoFrame = bt.lastSeenRightFrame = frameID;
            trackL.cookie = bt;
            trackR.cookie = bt;
            bundleViso.addObservation(currentLeft, bt, trackL.pixel.x, trackL.pixel.y);
            bundleViso.addObservation(currentRight, bt, trackR.pixel.x, trackR.pixel.y);
            visibleTracks.add(bt);
            total++;
        } else {
            // triangulation failed, drop track
            trackerLeft.dropTrack(trackL);
            trackerRight.dropTrack(trackR);
            bundleViso.tracks.removeTail();
        }
    }
    if (verbose != null)
        verbose.println("New Tracks: left=" + spawnedLeft.size() + " right=" + spawnedRight.size() + " stereo=" + total);
    // drop visual tracks that were not associated
    DogArray_I32 unassignedRight = assocL2R.getUnassociatedDestination();
    for (int i = 0; i < unassignedRight.size; i++) {
        int index = unassignedRight.get(i);
        trackerRight.dropTrack(spawnedRight.get(index));
    }
    DogArray_I32 unassignedLeft = assocL2R.getUnassociatedSource();
    for (int i = 0; i < unassignedLeft.size; i++) {
        int index = unassignedLeft.get(i);
        trackerLeft.dropTrack(spawnedLeft.get(index));
    }
    // Let the frame manager know how many tracks were just spawned
    frameManager.handleSpawnedTracks(trackerLeft, bundleViso.cameras.get(CAMERA_LEFT));
    frameManager.handleSpawnedTracks(trackerRight, bundleViso.cameras.get(CAMERA_RIGHT));
}

Example 42 with PointTrack

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

the class VisOdomDualTrackPnP method mutualTrackDrop.

/**
 * If a track was dropped in one image make sure it was dropped in the other image
 */
private void mutualTrackDrop() {
    int total = 0;
    for (PointTrack t : trackerLeft.getDroppedTracks(null)) {
        // lint:forbidden ignore_line
        TrackInfo bt = t.getCookie();
        trackerRight.dropTrack(bt.visualRight);
        // This tells the scene that it is no longer in the visual tracker
        bt.visualTrack = null;
        total++;
    }
    for (PointTrack t : trackerRight.getDroppedTracks(null)) {
        // lint:forbidden ignore_line
        TrackInfo bt = t.getCookie();
        if (bt.visualTrack != null) {
            trackerLeft.dropTrack(bt.visualTrack);
            bt.visualTrack = null;
            total++;
        }
    }
    if (verbose != null)
        verbose.println("Dropped Tracks Mutual: " + total);
}

Example 43 with PointTrack

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

the class VisOdomDualTrackPnP method describeSpawnedTracks.

/**
 * Given list of new visual tracks, describe the region around each track using a descriptor
 */
private void describeSpawnedTracks(T image, List<PointTrack> tracks, FastArray<Point2D_F64> points, DogArray<TD> descs) {
    describe.setImage(image);
    points.reset();
    descs.reset();
    for (int i = 0; i < tracks.size(); i++) {
        PointTrack t = tracks.get(i);
        // ignoring the return value. most descriptors never return false and the ones that due will rarely do so
        describe.process(t.pixel.x, t.pixel.y, 0, describeRadius, descs.grow());
        points.add(t.pixel);
    }
}

Example 44 with PointTrack

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

the class VisOdomDualTrackPnP method dropVisualTrack.

@Override
protected void dropVisualTrack(PointTrack left) {
    TrackInfo info = left.getCookie();
    PointTrack right = info.visualRight;
    trackerLeft.dropTrack(left);
    trackerRight.dropTrack(right);
}