Examples with Info boofcv.alg.sfm.d3.structure.SelectTracksInFrameForBundleAdjustment.Info used on opensource projects

Example 1 with Info

use of boofcv.alg.sfm.d3.structure.SelectTracksInFrameForBundleAdjustment.Info 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 2 with Info

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

Example 3 with Info

use of boofcv.alg.sfm.d3.structure.SelectTracksInFrameForBundleAdjustment.Info in project BoofCV by lessthanoptimal.

the class TestSelectTracksInFrameForBundleAdjustment method initializeGrid.

@Test
void initializeGrid() {
    var scene = new VisOdomBundleAdjustment<>(BTrack::new);
    var alg = new SelectTracksInFrameForBundleAdjustment(0xBEEF);
    alg.minTrackObservations = 1;
    scene.addCamera(new CameraPinholeBrown(0, 0, 0, 0, 0, width, height));
    for (int i = 0; i < 3; i++) {
        scene.addFrame(i);
    }
    BFrame targetFrame = scene.frames.get(2);
    // create enough tracks for there to be one in each cell
    connectFrames(1, 2, 200, scene);
    for (int i = 0; i < scene.tracks.size; i++) {
        BTrack track = scene.tracks.get(i);
        // pixel coordinates
        int x = (i * 10) % width;
        int y = 10 * ((i * 10) / width);
        // make sure it's false. should be already
        track.selected = false;
        track.findObservationBy(targetFrame).pixel.setTo(x, y);
    }
    // mark this one as active so that it isn't added to a cell. There should only be one empty cell
    scene.tracks.get(2).selected = true;
    // run it
    alg.initializeGrid(targetFrame, width, height, 10);
    // There should be one track in all but one cell
    for (int i = 0; i < alg.grid.cells.size; i++) {
        Info cell = alg.grid.cells.get(i);
        if (i != 2) {
            assertEquals(0, cell.alreadySelected);
            assertEquals(1, cell.unselected.size());
            assertSame(scene.tracks.get(i), cell.unselected.get(0));
        } else {
            assertEquals(1, cell.alreadySelected);
            assertEquals(0, cell.unselected.size());
        }
    }
}

Example 4 with Info

use of boofcv.alg.sfm.d3.structure.SelectTracksInFrameForBundleAdjustment.Info in project BoofCV by lessthanoptimal.

the class VisOdomMonoDepthPnP method estimateMotion.

/**
 * Estimates motion from the set of tracks and their 3D location
 *
 * @return true if successful.
 */
private boolean estimateMotion(List<PointTrack> active) {
    Point2Transform2_F64 pixelToNorm = cameraModels.get(0).pixelToNorm;
    Objects.requireNonNull(framePrevious).frame_to_world.invert(world_to_prev);
    // Create a list of observations for PnP
    // normalized image coordinates and 3D in the previous keyframe's reference frame
    observationsPnP.reset();
    for (int activeIdx = 0; activeIdx < active.size(); activeIdx++) {
        PointTrack pt = active.get(activeIdx);
        // Build the list of tracks which are currently visible
        initialVisible.add((Track) pt.cookie);
        // Extract info needed to estimate motion
        Point2D3D p = observationsPnP.grow();
        pixelToNorm.compute(pt.pixel.x, pt.pixel.y, p.observation);
        Track bt = pt.getCookie();
        // Go from world coordinates to the previous frame
        SePointOps_F64.transform(world_to_prev, bt.worldLoc, prevLoc4);
        // Go from homogenous coordinates into 3D coordinates
        PerspectiveOps.homogenousTo3dPositiveZ(prevLoc4, 1e8, 1e-7, p.location);
    }
    // estimate the motion up to a scale factor in translation
    if (!motionEstimator.process(observationsPnP.toList()))
        return false;
    Se3_F64 previous_to_current;
    if (refine != null) {
        previous_to_current = new Se3_F64();
        refine.fitModel(motionEstimator.getMatchSet(), motionEstimator.getModelParameters(), previous_to_current);
    } else {
        previous_to_current = motionEstimator.getModelParameters();
    }
    // Change everything back to the world frame
    previous_to_current.invert(current_to_previous);
    current_to_previous.concat(framePrevious.frame_to_world, frameCurrent.frame_to_world);
    return true;
}