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

Example 1 with BTrack

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

the class VisOdomBundlePnPBase method triangulateNotSelectedBundleTracks.

/**
 * Triangulate tracks which were not included in the optimization
 */
protected void triangulateNotSelectedBundleTracks() {
    final int minObservationsTriangulate = this.minObservationsTriangulate;
    for (int trackIdx = 0; trackIdx < bundleViso.tracks.size; trackIdx++) {
        final BTrack bt = bundleViso.tracks.data[trackIdx];
        // results will be unstable
        if (bt.selected || bt.observations.size < minObservationsTriangulate)
            continue;
        observationsNorm.reset();
        listOf_world_to_frame.reset();
        for (int obsIdx = 0; obsIdx < bt.observations.size; obsIdx++) {
            BObservation bo = bt.observations.get(obsIdx);
            CameraModel cm = cameraModels.get(bo.frame.camera.index);
            cm.pixelToNorm.compute(bo.pixel.x, bo.pixel.y, observationsNorm.grow());
            bo.frame.frame_to_world.invert(listOf_world_to_frame.grow());
        // NOTE: This invert could be cached. Doesn't need to be done a million times
        }
        // NOTE: If there is a homogenous metric triangulation added in the future replace this with that
        if (triangulateN.triangulate(observationsNorm.toList(), listOf_world_to_frame.toList(), found3D)) {
            bt.worldLoc.x = found3D.x;
            bt.worldLoc.y = found3D.y;
            bt.worldLoc.z = found3D.z;
            bt.worldLoc.w = 1.0;
        }
    }
}

Example 2 with BTrack

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BTrack 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 3 with BTrack

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BTrack 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 4 with BTrack

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

the class SelectTracksInFrameForBundleAdjustment method selectNewTracks.

/**
 * Selects new tracks such that it is uniform across the image. This takes in account the location of already
 * selected tracks.
 */
void selectNewTracks(List<BTrack> selected) {
    int total = 0;
    // this will result in a more even distribution.
    while (true) {
        int before = total;
        for (int cellIdx = 0; cellIdx < grid.cells.size && total < maxFeaturesPerFrame; cellIdx++) {
            Info cell = grid.cells.data[cellIdx];
            // and move on
            if (cell.alreadySelected > 0) {
                cell.alreadySelected--;
                total++;
                continue;
            }
            // nothing to select
            if (cell.unselected.isEmpty())
                continue;
            // Randomly select one of the available. Could probably do better but this is reasonable and "unbiased"
            int chosen = rand.nextInt(cell.unselected.size());
            BTrack bt = cell.unselected.remove(chosen);
            bt.selected = true;
            selected.add(bt);
            total++;
        }
        // See if exit condition has been meet
        if (before == total || total >= maxFeaturesPerFrame)
            break;
    }
}

Example 5 with BTrack

use of boofcv.alg.sfm.d3.structure.VisOdomBundleAdjustment.BTrack 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);
    }
}