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

Example 21 with BTrack

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

the class TestVisOdomBundleAdjustment method removeFrame.

@Test
void removeFrame() {
    VisOdomBundleAdjustment<BTrack> alg = createAlgSingleCamera();
    BFrame frameA = alg.addFrame(0);
    BFrame frameB = alg.addFrame(1);
    BTrack trackA = alg.addTrack(1, 2, 3, 4);
    BTrack trackB = alg.addTrack(1, 2, 3, 4);
    BTrack trackC = alg.addTrack(1, 2, 3, 4);
    alg.addObservation(frameA, trackA, 1, 2);
    alg.addObservation(frameA, trackC, 1, 3);
    alg.addObservation(frameB, trackB, 1, 4);
    alg.addObservation(frameB, trackC, 1, 5);
    alg.removeFrame(frameA, new ArrayList<>());
    assertEquals(1, alg.frames.size);
    assertEquals(2, alg.tracks.size);
    assertSame(frameB, alg.frames.get(0));
    assertTrue(alg.tracks.contains(trackB));
    assertTrue(alg.tracks.contains(trackC));
}

Example 22 with BTrack

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

the class TestVisOdomBundleAdjustment method addObservation.

@Test
void addObservation() {
    VisOdomBundleAdjustment<BTrack> alg = createAlgSingleCamera();
    BFrame frameA = alg.addFrame(0);
    BTrack trackA = alg.addTrack(1, 2, 3, 3);
    alg.addObservation(frameA, trackA, 5, 10);
    assertEquals(1, frameA.tracks.size);
    assertEquals(1, trackA.observations.size);
    BObservation obs = trackA.observations.get(0);
    assertSame(frameA, obs.frame);
    assertEquals(0.0, obs.pixel.distance(5, 10), UtilEjml.TEST_F64);
}

Example 23 with BTrack

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

the class TestTickTockKeyFrameManager method discardMultipleNewFrames.

/**
 * Tell it there are two new frames and see if it discard that many
 */
@Test
@Override
void discardMultipleNewFrames() {
    var tracker = new DummyTracker();
    VisOdomBundleAdjustment<BTrack> scene = createScene();
    int maxKeyFrames = 10;
    var alg = new TickTockKeyFrameManager();
    alg.keyframePeriod = 3;
    // add the initial set of frames
    for (int i = 0; i < 5; i++) {
        tracker.process(null);
        scene.addFrame(i * 2);
        scene.addFrame(i * 2 + 1);
    }
    for (int i = 0; i < 10; i++) {
        tracker.process(null);
        scene.addFrame((i + 5) * 2);
        scene.addFrame((i + 5) * 2 + 1);
        DogArray_I32 discard = alg.selectFramesToDiscard(tracker, maxKeyFrames, 2, scene);
        assertEquals(2, discard.size);
        long id = tracker.getFrameID();
        if (id % 3 == 0) {
            assertEquals(0, discard.get(0));
            assertEquals(1, discard.get(1));
        } else {
            assertEquals(10, discard.get(0));
            assertEquals(11, discard.get(1));
        }
        // remove the frame to make it realistic
        for (int idxDiscard = 0; idxDiscard < discard.size; idxDiscard++) {
            VisOdomBundleAdjustment.BFrame frame = scene.frames.get(discard.get(0));
            scene.removeFrame(frame, new ArrayList<>());
        }
    }
}

Example 24 with BTrack

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

the class TestTickTockKeyFrameManager method savePeriodically.

/**
 * See if it will periodically save the current frame
 */
@Test
void savePeriodically() {
    var tracker = new DummyTracker();
    VisOdomBundleAdjustment<BTrack> scene = createScene();
    var alg = new TickTockKeyFrameManager();
    alg.keyframePeriod = 3;
    // add the initial set of frames
    for (int i = 0; i < 5; i++) {
        tracker.process(null);
        scene.addFrame(i);
    }
    for (int i = 0; i < 10; i++) {
        tracker.process(null);
        scene.addFrame(i + 5);
        DogArray_I32 discard = alg.selectFramesToDiscard(tracker, maxKeyFrames, 1, scene);
        assertEquals(1, discard.size);
        long id = tracker.getFrameID();
        if (id % 3 == 0) {
            assertEquals(0, discard.get(0));
        } else {
            assertEquals(5, discard.get(0));
        }
        // remove the frame to make it realistic
        VisOdomBundleAdjustment.BFrame frame = scene.frames.get(discard.get(0));
        scene.removeFrame(frame, new ArrayList<>());
    }
}

Example 25 with BTrack

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

the class MaxGeoKeyFrameManager method selectOldToDiscard.

/**
 * Selects an older frame to discard. If a frame has zero features in common with the current frame it
 * will be selected. After that it scores frames based on how many features it has in common with the other
 * frames, excluding the current frame.
 *
 * @param totalDiscard How many need to be discarded
 */
protected void selectOldToDiscard(VisOdomBundleAdjustment<?> sba, int totalDiscard) {
    if (totalDiscard <= 0 || sba.frames.size < 2)
        return;
    frameToIndex.clear();
    for (int i = 0; i < sba.frames.size; i++) {
        frameToIndex.put(sba.frames.get(i).id, i);
    }
    // Create a histogram showing how observations connect the frames
    final int N = sba.frames.size;
    histogram.reshape(N, N);
    histogram.zero();
    // Skip the last row since it's not needed
    for (int frameIdxA = 0; frameIdxA < N - 1; frameIdxA++) {
        BFrame frame = sba.frames.get(frameIdxA);
        for (int trackIdx = 0; trackIdx < frame.tracks.size; trackIdx++) {
            BTrack track = frame.tracks.get(trackIdx);
            for (int obsIdx = 0; obsIdx < track.observations.size; obsIdx++) {
                BObservation o = track.observations.get(obsIdx);
                int frameIdxB = frameToIndex.get(o.frame.id);
                if (frameIdxA == frameIdxB)
                    continue;
                histogram.increment(frameIdxA, frameIdxB);
            }
        }
    }
    if (verbose != null)
        histogram.print("%4d");
    // See which keyframe the current frame has the best connection with
    // Most of the time it will be the previous frame, but if that was experienced a lot of motion blur it might
    // not be...
    int bestFrame = histogram.maximumRowIdx(N - 1);
    if (verbose != null)
        verbose.println("Frame with best connection to current " + bestFrame);
    // mark it so that it's skipped
    histogram.set(bestFrame, bestFrame, Integer.MAX_VALUE);
    for (int i = 0; i < totalDiscard; i++) {
        // Select the frame with the worst connection to the bestFrame. The reason the bestFrame is used and not
        // the current frame is that the current frame could be blurred too and might get discarded
        int lowestCount = Integer.MAX_VALUE;
        int worstIdx = -1;
        // N-1 to avoid the current frame
        for (int frameIdx = 0; frameIdx < N - 1; frameIdx++) {
            int connection = histogram.get(frameIdx, bestFrame);
            if (connection == Integer.MAX_VALUE)
                continue;
            // influence over the current frame's state and have no direct influence over bestFrame's state
            if (connection == 0) {
                if (verbose != null)
                    verbose.println("No connection index " + frameIdx);
                lowestCount = 0;
                worstIdx = frameIdx;
                break;
            }
            if (connection < lowestCount) {
                lowestCount = connection;
                worstIdx = frameIdx;
            }
        }
        if (verbose != null)
            verbose.println("Worst index " + worstIdx + "  count " + lowestCount);
        discardKeyIndices.add(worstIdx);
        // Mark the worst keyframe so it won't be selected again
        histogram.set(worstIdx, bestFrame, Integer.MAX_VALUE);
    }
}