Examples with BFrame boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BFrame used on opensource projects

Example 1 with BFrame

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BFrame in project BoofCV by lessthanoptimal.

the class VisOdomDualTrackPnP method process.

/**
 * Updates motion estimate using the stereo pair.
 *
 * @param left Image from left camera
 * @param right Image from right camera
 * @return true if motion estimate was updated and false if not
 */
public boolean process(T left, T right) {
    if (verbose != null) {
        verbose.println("----------- Process --------------");
        verbose.println("Scene: Frames=" + bundleViso.frames.size + " Tracks=" + bundleViso.tracks.size);
        for (int frameIdx = 0; frameIdx < bundleViso.frames.size; frameIdx++) {
            BFrame bf = bundleViso.frames.get(frameIdx);
            verbose.printf("   frame[%2d] cam=%d tracks=%d\n", frameIdx, bf.camera.index, bf.tracks.size);
        }
    }
    this.inputLeft = left;
    this.inputRight = right;
    // =============================================================================================
    // ========== Visually track features
    double time0 = System.nanoTime();
    inlierTracks.clear();
    visibleTracks.clear();
    initialVisible.clear();
    candidates.clear();
    // Create a new frame for the current image
    currentLeft = bundleViso.addFrame(CAMERA_LEFT, trackerLeft.getFrameID());
    currentRight = bundleViso.addFrame(CAMERA_RIGHT, trackerRight.getFrameID());
    // TODO in the future when bundle adjustment supports rigid relationships between two views use that here
    // Track objects given the new images
    trackerLeft.process(left);
    trackerRight.process(right);
    double time1 = System.nanoTime();
    // ========== Initialize VO from the first image and return
    if (first) {
        first = false;
        frameManager.initialize(bundleViso.cameras);
        addNewTracks();
        // The left camera is the world frame right now
        currentLeft.frame_to_world.reset();
        currentRight.frame_to_world.setTo(right_to_left);
        return true;
    }
    // This will be used as a reference for motion estimation
    // tail(3) since the two visible frames (left + right) where just added
    previousLeft = bundleViso.frames.getTail(3);
    // If one tracker dropped a track then drop the same track in the other camera
    mutualTrackDrop();
    // Find tracks which pass a geometric test and put into candidates list
    selectCandidateStereoTracks();
    // Robustly estimate motion using features in candidates list
    if (!estimateMotion()) {
        if (verbose != null)
            verbose.println("!!! Motion Failed !!!");
        removedBundleTracks.clear();
        bundleViso.removeFrame(currentRight, removedBundleTracks);
        bundleViso.removeFrame(currentLeft, removedBundleTracks);
        return false;
    }
    addInlierObservationsToScene();
    removeOldUnusedVisibleTracks();
    // =============================================================================================
    // ========== Refine the scene's state estimate
    double time2 = System.nanoTime();
    optimizeTheScene();
    double time3 = System.nanoTime();
    // =============================================================================================
    // ========== Perform maintenance by dropping elements from the scene
    dropBadBundleTracks();
    long time4 = System.nanoTime();
    boolean droppedCurrentFrame = performKeyFrameMaintenance(trackerLeft, 2);
    long time5 = System.nanoTime();
    if (!droppedCurrentFrame) {
        if (verbose != null)
            verbose.println("Saving new key frames");
        // We are keeping the current frame! Spawn new tracks inside of it
        addNewTracks();
    }
    long time6 = System.nanoTime();
    // =============================================================================================
    // ========== Summarize profiling results
    timeTracking = (time1 - time0) * 1e-6;
    timeEstimate = (time2 - time1) * 1e-6;
    timeBundle = (time3 - time2) * 1e-6;
    timeDropUnused = (time4 - time3) * 1e-6;
    timeSceneMaintenance = (time5 - time4) * 1e-6;
    timeSpawn = (time6 - time5) * 1e-6;
    if (profileOut != null) {
        double timeTotal = (time6 - time0) * 1e-6;
        profileOut.printf("TIME: TRK %5.1f Est %5.1f Bun %5.1f DU %5.1f Scene %5.1f Spn  %5.1f TOTAL %5.1f\n", timeTracking, timeEstimate, timeBundle, timeDropUnused, timeSceneMaintenance, timeSpawn, timeTotal);
    }
    return true;
}

Example 2 with BFrame

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BFrame in project BoofCV by lessthanoptimal.

the class VisOdomBundlePnPBase method dropFramesFromScene.

/**
 * Removes the frames listed from the scene
 *
 * @param dropFrameIndexes List of indexes to drop. Sorted from lowest to highest
 */
protected void dropFramesFromScene(DogArray_I32 dropFrameIndexes) {
    for (int i = dropFrameIndexes.size - 1; i >= 0; i--) {
        // indexes are ordered from lowest to highest, so you can remove frames without
        // changing the index in the list
        BFrame frameToDrop = bundleViso.frames.get(dropFrameIndexes.get(i));
        // System.out.println("Dropping frame ID "+frameToDrop.id);
        // update data structures
        bundleViso.removeFrame(frameToDrop, removedBundleTracks);
        // These tracks were visually being tracked and were removed. So drop them from the visual tracker
        for (int removeIdx = 0; removeIdx < removedBundleTracks.size(); removeIdx++) {
            dropVisualTrack(removedBundleTracks.get(removeIdx));
        }
    }
}

Example 3 with BFrame

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BFrame in project BoofCV by lessthanoptimal.

the class VisOdomBundlePnPBase method dropTracksNotVisibleAndTooFewObservations.

/**
 * Drop tracks which are no longer being visually tracked and have less than two observations. In general
 * 3 observations is much more stable than two and less prone to be a false positive.
 */
protected void dropTracksNotVisibleAndTooFewObservations() {
    final int minObservationsNotVisible = this.minObservationsNotVisible;
    // iteration through track lists in reverse order because of removeSwap()
    for (int tidx = bundleViso.tracks.size - 1; tidx >= 0; tidx--) {
        BTrack bt = bundleViso.tracks.get(tidx);
        if (bt.visualTrack == null && bt.observations.size < minObservationsNotVisible) {
            // Mark it as dropped. Formally remove it in the next loop
            bt.observations.reset();
            bundleViso.tracks.removeSwap(tidx);
        // System.out.println("drop old bt="+bt.id+" vt=NONE");
        }
    }
    // Need to remove the dropped tracks from each frame that saw them.
    for (int fidx = 0; fidx < bundleViso.frames.size; fidx++) {
        BFrame bf = bundleViso.frames.get(fidx);
        for (int tidx = bf.tracks.size - 1; tidx >= 0; tidx--) {
            BTrack bt = bf.tracks.get(tidx);
            if (bt.observations.size == 0) {
                bf.tracks.removeSwap(tidx);
            // System.out.println("removing track="+bt.id+" from frame="+bf.id);
            }
        }
    }
}

Example 4 with BFrame

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BFrame in project BoofCV by lessthanoptimal.

the class VisOdomBundlePnPBase method dropBadBundleTracks.

/**
 * Remove tracks with large errors and impossible geometry. For now it just removes tracks behind a camera.
 */
void dropBadBundleTracks() {
    int totalBehind = 0;
    // Go through each frame and look for tracks which are bad
    for (int fidx = 0; fidx < bundleViso.frames.size; fidx++) {
        BFrame bf = bundleViso.frames.get(fidx);
        bf.frame_to_world.invert(world_to_frame);
        for (int tidx = bf.tracks.size - 1; tidx >= 0; tidx--) {
            BTrack bt = bf.tracks.get(tidx);
            // Remove from frame if it was already marked for removal
            if (bt.observations.size == 0) {
                continue;
            }
            // test to see if the feature is behind the camera while avoiding divided by zero errors
            SePointOps_F64.transform(world_to_frame, bt.worldLoc, cameraLoc);
            if (PerspectiveOps.isBehindCamera(cameraLoc)) {
                totalBehind++;
                // this marks it for removal later on
                bt.observations.reset();
            // System.out.println("Dropping bad track. id="+bt.id+" z="+(cameraLoc.z/cameraLoc.w));
            }
        // Isn't it a bit excessive to drop the entire track if it's bad in just one frame?
        // TODO test to see if residual is excessively large
        }
    }
    // Remove it from the master tracks list
    totalDroppedTracksBadBundle = bundleViso.tracks.size;
    for (int tidx = bundleViso.tracks.size - 1; tidx >= 0; tidx--) {
        BTrack bt = bundleViso.tracks.get(tidx);
        if (bt.observations.size == 0) {
            if (bt.id == -1) {
                throw new RuntimeException("BUG! Dropping a track that was never initialized");
            }
            bundleViso.tracks.removeSwap(tidx);
        }
    }
    // the delta is the number of dropped tracks
    totalDroppedTracksBadBundle -= bundleViso.tracks.size;
    if (verbose != null)
        verbose.printf("drop bundle: total=%d {behind=%d}\n", totalBehind, totalDroppedTracksBadBundle);
    // if it was good in an earlier one
    for (int fidx = 0; fidx < bundleViso.frames.size; fidx++) {
        BFrame bf = bundleViso.frames.get(fidx);
        for (int tidx = bf.tracks.size - 1; tidx >= 0; tidx--) {
            BTrack bt = bf.tracks.get(tidx);
            if (bt.observations.size == 0) {
                // System.out.println("  Removing track from frame: "+bt.id);
                bf.tracks.removeSwap(tidx);
                if (bt.visualTrack != null) {
                    dropVisualTrack(bt.visualTrack);
                    bt.visualTrack = null;
                }
            }
        }
    }
}

Example 5 with BFrame

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BFrame in project BoofCV by lessthanoptimal.

the class SelectTracksInFrameForBundleAdjustment method initializeGrid.

/**
 * Initializes the grid data structure. Counts number of already selected tracks and adds unselected
 * tracks to the list. A track is only considered for selection if it has the minimum number of observations.
 * Otherwise it's likely to be a false positive.
 *
 * @param targetLength See {@link ImageGrid#initialize(int, int, int)}
 */
void initializeGrid(BFrame frame, int imageWidth, int imageHeight, int targetLength) {
    grid.initialize(targetLength, imageWidth, imageHeight);
    final FastArray<BTrack> tracks = frame.tracks;
    for (int trackIdx = 0; trackIdx < tracks.size; trackIdx++) {
        BTrack bt = tracks.get(trackIdx);
        VisOdomBundleAdjustment.BObservation o = bt.findObservationBy(frame);
        if (// TODO Running mono-klt generated this exception with r=1
        o == null)
            throw new RuntimeException("BUG! track in frame not observed by frame");
        Info cell = grid.getCellAtPixel((int) o.pixel.x, (int) o.pixel.y);
        if (bt.selected)
            cell.alreadySelected++;
        else if (bt.observations.size >= minTrackObservations)
            cell.unselected.add(bt);
    }
}