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

Example 16 with BTrack

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

the class VisOdomMonoDepthPnP method spawnNewTracksForNewKeyFrame.

/**
 * Detects new features and computes their 3D coordinates
 *
 * @param visibleTracks newly spawned tracks are added to this list
 */
private void spawnNewTracksForNewKeyFrame(List<Track> visibleTracks) {
    // System.out.println("addNewTracks() current frame="+frameCurrent.id);
    long frameID = tracker.getFrameID();
    int totalRejected = 0;
    tracker.spawnTracks();
    List<PointTrack> spawned = tracker.getNewTracks(null);
    // estimate 3D coordinate using stereo vision
    for (PointTrack pt : spawned) {
        // discard point if it can't localized
        if (!pixelTo3D.process(pt.pixel.x, pt.pixel.y)) {
            // System.out.println("Dropped pixelTo3D  tt="+pt.featureId);
            totalRejected++;
            tracker.dropTrack(pt);
        } else {
            if (bundleViso.findByTrackerTrack(pt) != null) {
                // System.out.println("BUG! Tracker recycled... bt="+btrack.id+" tt="+t.featureId);
                throw new RuntimeException("BUG! Recycled tracker track too early tt=" + pt.featureId);
            }
            // Save the track's 3D location and add it to the current frame
            Track btrack = bundleViso.addTrack(pixelTo3D.getX(), pixelTo3D.getY(), pixelTo3D.getZ(), pixelTo3D.getW());
            btrack.lastUsed = frameID;
            btrack.visualTrack = pt;
            btrack.id = pt.featureId;
            pt.cookie = btrack;
            // System.out.println("new track bt="+btrack.id+" tt.id="+t.featureId);
            // Convert the location from local coordinate system to world coordinates
            SePointOps_F64.transform(frameCurrent.frame_to_world, btrack.worldLoc, btrack.worldLoc);
            // keep the scale of floats manageable and normalize the vector to have a norm of 1
            // Homogeneous coordinates so the distance is determined by the ratio of w and other elements
            btrack.worldLoc.normalize();
            bundleViso.addObservation(frameCurrent, btrack, pt.pixel.x, pt.pixel.y);
            // for (int i = 0; i < visibleTracks.size(); i++) {
            // if( visibleTracks.get(i).visualTrack == t )
            // throw new RuntimeException("Bug. Adding duplicate track: "+t.featureId);
            // }
            visibleTracks.add(btrack);
        }
    }
    if (verbose != null)
        verbose.printf("spawn: new=%d rejected=%d\n", spawned.size() - totalRejected, totalRejected);
}

Example 17 with BTrack

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

the class ChecksVisOdomKeyFrameManager method discardMultipleFramesWithNothing.

@Test
void discardMultipleFramesWithNothing() {
    VisOdomBundleAdjustment<BTrack> scene = createScene();
    var tracker = new TestMaxGeoKeyFrameManager.DummyTracker();
    tracker.activeTracks = 50;
    tracker.maxSpawn = 60;
    VisOdomKeyFrameManager alg = createFrameManager();
    alg.initialize(scene.cameras);
    for (int i = 0; i < 5; i++) {
        scene.addFrame(i);
        DogArray_I32 discard = alg.selectFramesToDiscard(tracker, 5, 1, scene);
        assertEquals(0, discard.size);
        alg.handleSpawnedTracks(tracker, scene.cameras.getTail());
    }
    // connect the two most recent frames
    scene.addFrame(5);
    connectFrames(4, 5, 10, scene);
    // tell it to only keep 3
    DogArray_I32 discard = alg.selectFramesToDiscard(tracker, 3, 1, scene);
    // there should only be 3 dropped in this order
    assertEquals(3, discard.size);
    for (int i = 0; i < 3; i++) {
        assertEquals(i, discard.get(i));
    }
}

Example 18 with BTrack

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

the class TestSelectTracksInFrameForBundleAdjustment method minimal_all.

/**
 * Basic test. Each track is visible in every frame at the same location.
 */
@Test
void minimal_all() {
    var scene = new VisOdomBundleAdjustment<>(BTrack::new);
    var alg = new SelectTracksInFrameForBundleAdjustment(0xBEEF);
    // makes the math easier
    alg.configUniform.regionScaleFactor = 1.0;
    alg.maxFeaturesPerFrame = 200;
    var selected = new ArrayList<BTrack>();
    scene.addCamera(new CameraPinholeBrown(0, 0, 0, 0, 0, width, height));
    for (int i = 0; i < 5; i++) {
        scene.addFrame(i);
    }
    for (int i = 0; i < 200; i++) {
        scene.tracks.grow().id = i;
    }
    for (int i = 0; i < scene.frames.size; i++) {
        BFrame frame = scene.frames.get(i);
        for (int j = 0; j < 200; j++) {
            BTrack track = scene.tracks.get(j);
            frame.tracks.add(track);
            // pixel coordinates
            int x = (i * 10) % width;
            int y = 10 * ((i * 10) / width);
            VisOdomBundleAdjustment.BObservation o = track.observations.grow();
            o.frame = frame;
            o.pixel.setTo(x, y);
        }
    }
    alg.selectTracks(scene, selected);
    assertEquals(200, selected.size());
}

Example 19 with BTrack

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

the class TestVisOdomBundleAdjustment method createPerfectScene.

private void createPerfectScene(VisOdomBundleAdjustment<BTrack> vsba) {
    vsba.reset();
    List<Point3D_F64> cloud = UtilPoint3D_F64.random(new Point3D_F64(0, 0, 1.5), -1.5, 1.5, -0.5, 0.5, -0.2, 0.2, 500, rand);
    LensDistortionPinhole distortion = new LensDistortionPinhole(pinhole);
    Point2Transform2_F64 n2n = distortion.distort_F64(false, false);
    vsba.addCamera(pinhole);
    // 3D point in view reference frame
    Point3D_F64 Xv = new Point3D_F64();
    // normalized image coordinate
    Point2D_F64 n = new Point2D_F64();
    // pixel coordinate
    Point2D_F64 p = new Point2D_F64();
    for (int i = 0; i < cloud.size(); i++) {
        Point3D_F64 X = cloud.get(i);
        vsba.addTrack(X.x, X.y, X.z, 1.0).hasBeenInlier = true;
    }
    for (int viewidx = 0; viewidx < 5; viewidx++) {
        BFrame frame = vsba.addFrame(viewidx);
        frame.frame_to_world.setTo(-1 + 2.0 * viewidx / 4.0, 0, 0, EulerType.XYZ, rand.nextGaussian() * 0.1, 0, 0);
        // add visible features to each view
        for (int i = 0; i < cloud.size(); i++) {
            Point3D_F64 X = cloud.get(i);
            frame.frame_to_world.transformReverse(X, Xv);
            if (Xv.z <= 0)
                continue;
            n2n.compute(Xv.x / Xv.z, Xv.y / Xv.z, n);
            PerspectiveOps.convertNormToPixel(pinhole, n.x, n.y, p);
            if (!pinhole.isInside(p.x, p.y))
                continue;
            BTrack track = vsba.tracks.get(i);
            vsba.addObservation(frame, track, p.x, p.y);
        }
    }
}

Example 20 with BTrack

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

the class TestVisOdomBundleAdjustment method createAlgSingleCamera.

private VisOdomBundleAdjustment<BTrack> createAlgSingleCamera() {
    var alg = new VisOdomBundleAdjustment<>(BTrack::new);
    alg.addCamera(new CameraPinholeBrown(0, 0, 0, 0, 0, 100, 200));
    return alg;
}