Examples with AssociatedIndex boofcv.struct.feature.AssociatedIndex used on opensource projects

Example 21 with AssociatedIndex

use of boofcv.struct.feature.AssociatedIndex in project BoofCV by lessthanoptimal.

the class AssociationPanel method setAssociation.

public synchronized void setAssociation(List<Point2D_F64> leftPts, List<Point2D_F64> rightPts, FastQueue<AssociatedIndex> matches) {
    List<Point2D_F64> allLeft = new ArrayList<>();
    List<Point2D_F64> allRight = new ArrayList<>();
    assocLeft = new int[matches.size()];
    assocRight = new int[matches.size()];
    for (int i = 0; i < matches.size(); i++) {
        AssociatedIndex a = matches.get(i);
        allLeft.add(leftPts.get(a.src));
        allRight.add(rightPts.get(a.dst));
        assocLeft[i] = i;
        assocRight[i] = i;
    }
    setLocation(allLeft, allRight);
    // assocLeft = new int[ leftPts.size() ];
    // assocRight = new int[ rightPts.size() ];
    // 
    // for( int i = 0; i < assocLeft.length; i++ )
    // assocLeft[i] = -1;
    // for( int i = 0; i < assocRight.length; i++ )
    // assocRight[i] = -1;
    // 
    // for( int i = 0; i < matches.size; i++ ) {
    // AssociatedIndex a = matches.get(i);
    // assocLeft[a.src] = a.dst;
    // assocRight[a.dst] = a.src;
    // }
    Random rand = new Random(234);
    colors = new Color[leftPts.size()];
    for (int i = 0; i < colors.length; i++) {
        colors[i] = new Color(rand.nextInt() | 0xFF000000);
    }
}

Example 22 with AssociatedIndex

use of boofcv.struct.feature.AssociatedIndex in project BoofCV by lessthanoptimal.

the class CombinedTrackerScalePoint method spawnTracksFromDetected.

/**
 * From the found interest points create new tracks.  Tracks are only created at points
 * where there are no existing tracks.
 *
 * Note: Must be called after {@link #associateAllToDetected}.
 */
public void spawnTracksFromDetected() {
    // mark detected features with no matches as available
    FastQueue<AssociatedIndex> matches = associate.getMatches();
    int N = detector.getNumberOfFeatures();
    for (int i = 0; i < N; i++) associated[i] = false;
    for (AssociatedIndex i : matches.toList()) {
        associated[i.dst] = true;
    }
    // spawn new tracks for unassociated detected features
    for (int i = 0; i < N; i++) {
        if (associated[i])
            continue;
        Point2D_F64 p = detector.getLocation(i);
        TD d = detectedDesc.get(i);
        CombinedTrack<TD> track;
        if (tracksUnused.size() > 0) {
            track = tracksUnused.pop();
        } else {
            track = new CombinedTrack<>();
            track.desc = detector.createDescription();
            track.track = trackerKlt.createNewTrack();
        }
        // create the descriptor for tracking
        trackerKlt.setDescription((float) p.x, (float) p.y, track.track);
        // set track ID and location
        track.featureId = totalTracks++;
        track.desc.setTo(d);
        track.set(p);
        // update list of active tracks
        tracksPureKlt.add(track);
        tracksSpawned.add(track);
    }
}

Example 23 with AssociatedIndex

use of boofcv.struct.feature.AssociatedIndex in project BoofCV by lessthanoptimal.

the class CombinedTrackerScalePoint method associateAllToDetected.

/**
 * Associate all tracks in any state to the latest observations.  If a dormant track is associated it
 * will be reactivated.  If a reactivated track is associated it's state will be updated.  PureKLT
 * tracks are left unmodified.
 */
public void associateAllToDetected() {
    // initialize data structures
    List<CombinedTrack<TD>> all = new ArrayList<>();
    all.addAll(tracksReactivated);
    all.addAll(tracksDormant);
    all.addAll(tracksPureKlt);
    int numTainted = tracksReactivated.size() + tracksDormant.size();
    tracksReactivated.clear();
    tracksDormant.clear();
    // detect features
    detector.detect(input);
    // associate features
    associateToDetected(all);
    FastQueue<AssociatedIndex> matches = associate.getMatches();
    // See which features got respawned and which ones are made dormant
    for (int i = 0; i < numTainted; i++) {
        associated[i] = false;
    }
    for (AssociatedIndex a : matches.toList()) {
        // don't mess with pure-KLT tracks
        if (a.src >= numTainted)
            continue;
        CombinedTrack<TD> t = all.get(a.src);
        t.set(detector.getLocation(a.dst));
        trackerKlt.setDescription((float) t.x, (float) t.y, t.track);
        tracksReactivated.add(t);
        associated[a.src] = true;
    }
    for (int i = 0; i < numTainted; i++) {
        if (!associated[i]) {
            tracksDormant.add(all.get(i));
        }
    }
}

Example 24 with AssociatedIndex

use of boofcv.struct.feature.AssociatedIndex in project BoofCV by lessthanoptimal.

the class VisOdomDualTrackPnP method addNewTracks.

/**
 * Spawns tracks in each image and associates features together.
 */
private void addNewTracks() {
    trackerLeft.spawnTracks();
    trackerRight.spawnTracks();
    List<PointTrack> newLeft = trackerLeft.getNewTracks(null);
    List<PointTrack> newRight = trackerRight.getNewTracks(null);
    // get a list of new tracks and their descriptions
    addNewToList(inputLeft, newLeft, pointsLeft, descLeft);
    addNewToList(inputRight, newRight, pointsRight, descRight);
    // associate using L2R
    assocL2R.setSource(pointsLeft, descLeft);
    assocL2R.setDestination(pointsRight, descRight);
    assocL2R.associate();
    FastQueue<AssociatedIndex> matches = assocL2R.getMatches();
    // storage for the triangulated location in the camera frame
    Point3D_F64 cameraP3 = new Point3D_F64();
    for (int i = 0; i < matches.size; i++) {
        AssociatedIndex m = matches.get(i);
        PointTrack trackL = newLeft.get(m.src);
        PointTrack trackR = newRight.get(m.dst);
        // declare additional track information stored in each track.  Tracks can be recycled so it
        // might not always need to be declared
        LeftTrackInfo infoLeft = trackL.getCookie();
        if (infoLeft == null)
            trackL.cookie = infoLeft = new LeftTrackInfo();
        RightTrackInfo infoRight = trackR.getCookie();
        if (infoRight == null)
            trackR.cookie = infoRight = new RightTrackInfo();
        Stereo2D3D p2d3d = infoLeft.location;
        // convert pixel observations into normalized image coordinates
        leftImageToNorm.compute(trackL.x, trackL.y, p2d3d.leftObs);
        rightImageToNorm.compute(trackR.x, trackR.y, p2d3d.rightObs);
        // triangulate 3D coordinate in the current camera frame
        if (triangulate.triangulate(p2d3d.leftObs, p2d3d.rightObs, leftToRight, cameraP3)) {
            // put the track into the current keyframe coordinate system
            SePointOps_F64.transform(currToKey, cameraP3, p2d3d.location);
            // save a reference to the matching track in the right camera frame
            infoLeft.right = trackR;
            infoLeft.lastConsistent = infoLeft.lastInlier = tick;
            infoRight.left = trackL;
        } else {
            // triangulation failed, drop track
            trackerLeft.dropTrack(trackL);
            // TODO need way to mark right tracks which are unassociated after this loop
            throw new RuntimeException("This special case needs to be handled!");
        }
    }
    // drop tracks that were not associated
    GrowQueue_I32 unassignedRight = assocL2R.getUnassociatedDestination();
    for (int i = 0; i < unassignedRight.size; i++) {
        int index = unassignedRight.get(i);
        // System.out.println(" unassigned right "+newRight.get(index).x+" "+newRight.get(index).y);
        trackerRight.dropTrack(newRight.get(index));
    }
    GrowQueue_I32 unassignedLeft = assocL2R.getUnassociatedSource();
    for (int i = 0; i < unassignedLeft.size; i++) {
        int index = unassignedLeft.get(i);
        trackerLeft.dropTrack(newLeft.get(index));
    }
// System.out.println("Total left "+trackerLeft.getAllTracks(null).size()+"  right "+trackerRight.getAllTracks(null).size());
// System.out.println("Associated: "+matches.size+" new left "+newLeft.size()+" new right "+newRight.size());
// System.out.println("New Tracks: Total: Left "+trackerLeft.getAllTracks(null).size()+" right "+
// trackerRight.getAllTracks(null).size());
// List<PointTrack> temp = trackerLeft.getActiveTracks(null);
// for( PointTrack t : temp ) {
// if( t.cookie == null )
// System.out.println("BUG!");
// }
// temp = trackerRight.getActiveTracks(null);
// for( PointTrack t : temp ) {
// if( t.cookie == null )
// System.out.println("BUG!");
// }
}