Examples with SceneStructureMetric boofcv.abst.geo.bundle.SceneStructureMetric used on opensource projects

Example 6 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class RefineMetricWorkingGraph method initializeDataStructures.

/**
 * Initialized several data structures and resets it into the initial state
 */
void initializeDataStructures(LookUpSimilarImages dbSimilar, SceneWorkingGraph graph) {
    viewToIntegerID.clear();
    listPixelToNorm.clear();
    listNormToPixel.clear();
    final SceneStructureMetric structure = metricSba.structure;
    final SceneObservations observations = metricSba.observations;
    // Initialize the structure, but save initializing the points for later
    structure.initialize(graph.listCameras.size, graph.listViews.size(), 0);
    // Go through each view and load the observations then add them to the scene, but don't specify which
    // 3D point they are observing yet
    observations.initialize(graph.listViews.size());
    // First add cameras to the structure
    for (int cameraIdx = 0; cameraIdx < graph.listCameras.size; cameraIdx++) {
        SceneWorkingGraph.Camera wcam = graph.listCameras.get(cameraIdx);
        structure.setCamera(cameraIdx, false, wcam.intrinsic);
    }
    // add views next
    for (int viewIdx = 0; viewIdx < graph.listViews.size(); viewIdx++) {
        SceneWorkingGraph.View wview = graph.listViews.get(viewIdx);
        SceneObservations.View oview = observations.getView(viewIdx);
        viewToIntegerID.put(wview.pview.id, viewIdx);
        createProjectionModel(graph.getViewCamera(wview).intrinsic);
        // Add all observations in this view to the SBA observations.
        // Observations that are not assigned to a 3D point will be pruned later on. Much easier this way.
        oview.resize(wview.pview.totalObservations);
        dbSimilar.lookupPixelFeats(wview.pview.id, pixels);
        BoofMiscOps.checkEq(pixels.size, wview.pview.totalObservations);
        // The camera model assumes the principle point is (0,0) and this is done by assuming it's the image center
        SceneWorkingGraph.Camera camera = graph.getViewCamera(wview);
        float cx = (float) camera.prior.cx;
        float cy = (float) camera.prior.cy;
        // specify the observation pixel coordinates but not which 3D feature is matched to the observation
        for (int obsIdx = 0; obsIdx < pixels.size; obsIdx++) {
            Point2D_F64 p = pixels.get(obsIdx);
            oview.setPixel(obsIdx, (float) (p.x - cx), (float) (p.y - cy));
        }
        // Add this view to the graph and it's location
        structure.setView(viewIdx, camera.localIndex, viewIdx == 0, wview.world_to_view);
    }
}

Example 7 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class RefineMetricWorkingGraph method assignKnown3DToUnassignedObs.

/**
 * Assigns world 3D features that were already matched to others in the inlier view set to the
 * unassigned observations.
 *
 * Part of the idea behind only associating an observation with a 3D feature if the preprojection error is less
 * than some value is that tracks can drift from one object to another, but it's useful to save both.
 *
 * @param inlierIdx Index of the observations. All observations with this index point to the same 3D feature
 */
void assignKnown3DToUnassignedObs(SceneWorkingGraph graph, SceneWorkingGraph.InlierInfo inliers, int inlierIdx) {
    final SceneStructureMetric structure = metricSba.structure;
    final SceneObservations observations = metricSba.observations;
    // Go through all the views/observations which have yet to be assigned a 3D feature
    for (int unassignedIdx = unassigned.size - 1; unassignedIdx >= 0; unassignedIdx--) {
        int whichViewInliers = unassigned.get(unassignedIdx);
        int whichViewID = sceneViewIntIds.get(whichViewInliers);
        // Lookup the pixel observation in the view
        int viewObsIdx = inliers.observations.get(whichViewInliers).get(inlierIdx);
        observations.getView(whichViewID).getPixel(viewObsIdx, pixelObserved);
        // look up scene information for this view
        SceneWorkingGraph.View wview = graph.listViews.get(whichViewID);
        Point2Transform2_F64 normToPixels = listNormToPixel.get(whichViewID);
        // See which 3D feature best matches this observation
        double bestScore = maxReprojectionErrorPixel * maxReprojectionErrorPixel;
        int bestId = -1;
        for (int knownIdx = 0; knownIdx < featureIdx3D.size; knownIdx++) {
            int featureId = featureIdx3D.get(knownIdx);
            // If this feature has already been assigned to this view skip over it
            if (structure.points.get(featureId).views.contains(whichViewID))
                continue;
            structure.getPoints().get(featureId).get(world3D);
            double error = computeReprojectionError(wview.world_to_view, normToPixels, pixelObserved, world3D);
            if (error <= bestScore) {
                bestScore = error;
                bestId = featureId;
            }
        }
        if (bestId == -1) {
            if (verbose != null)
                verbose.println("Not matching. Reprojection error too large view=" + whichViewID);
            continue;
        }
        // assign this scene feature to this observation
        observations.getView(whichViewID).safeAssignToFeature(viewObsIdx, bestId);
        structure.connectPointToView(bestId, whichViewID);
        // Remove it since it has been assigned. This is also why we iterate in reverse
        unassigned.removeSwap(unassignedIdx);
    }
}

Example 8 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class VisOdomStereoQuadPnP method performBundleAdjustment.

/**
 * Optimize cameras and feature locations at the same time
 */
private void performBundleAdjustment(Se3_F64 key_to_curr) {
    if (bundle.configConverge.maxIterations <= 0)
        return;
    // Must only process inlier tracks here
    inliers.clear();
    for (int trackIdx = 0; trackIdx < consistentTracks.size(); trackIdx++) {
        TrackQuad t = consistentTracks.get(trackIdx);
        if (t.leftCurrIndex != -1 && t.inlier)
            inliers.add(t);
    }
    // Copy the scene into a data structure bundle adjustment understands
    SceneStructureMetric structure = bundle.getStructure();
    SceneObservations observations = bundle.getObservations();
    observations.initialize(4);
    structure.initialize(2, 4, inliers.size());
    // known relative view relating left to right cameras
    int baseline = structure.addMotion(true, left_to_right);
    structure.setCamera(0, true, stereoParameters.left);
    structure.setCamera(1, true, stereoParameters.right);
    // view[0].left
    structure.setView(0, 0, true, listWorldToView.get(0));
    // view[0].right
    structure.setView(1, 1, baseline, 0);
    // view[1].left
    structure.setView(2, 0, false, listWorldToView.get(2));
    // view[1].right
    structure.setView(3, 1, baseline, 2);
    for (int trackIdx = 0; trackIdx < inliers.size(); trackIdx++) {
        TrackQuad t = inliers.get(trackIdx);
        Point3D_F64 X = t.X;
        structure.setPoint(trackIdx, X.x, X.y, X.z);
        observations.getView(0).add(trackIdx, (float) t.v0.x, (float) t.v0.y);
        observations.getView(1).add(trackIdx, (float) t.v1.x, (float) t.v1.y);
        observations.getView(2).add(trackIdx, (float) t.v2.x, (float) t.v2.y);
        observations.getView(3).add(trackIdx, (float) t.v3.x, (float) t.v3.y);
    }
    if (!bundle.process())
        return;
    // Update the state of tracks and the current views
    for (int trackIdx = 0; trackIdx < inliers.size(); trackIdx++) {
        TrackQuad t = inliers.get(trackIdx);
        structure.points.get(trackIdx).get(t.X);
    }
    // Reminder: World here refers to key left view
    key_to_curr.setTo(structure.getParentToView(2));
}

Example 9 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class TestColorizeMultiViewStereoResults method simple_processScenePoints.

/**
 * Two points seen in the two views. Which view is first depends on the point. See if the expected point
 * has the expected color.
 */
@Test
void simple_processScenePoints() {
    // scene with two views that are identical
    var scene = new SceneStructureMetric(true);
    scene.initialize(1, 2, 2);
    scene.setCamera(0, true, new CameraPinhole(200, 200, 0, width / 2, height / 2, 0, 0));
    scene.setView(0, 0, true, SpecialEuclideanOps_F64.eulerXyz(0, 0, 0, 0, 0, 0, null));
    scene.setView(1, 0, true, SpecialEuclideanOps_F64.eulerXyz(0, 0, 0, 0, 0, 0, null));
    // only difference between the points are the order of their views
    scene.setPoint(0, 0.01, -0.01, 2.0, 0.99);
    scene.setPoint(1, 0.01, -0.01, 2.0, 0.99);
    scene.points.get(0).views.addAll(DogArray_I32.array(0, 1));
    scene.points.get(1).views.addAll(DogArray_I32.array(1, 0));
    var alg = new ColorizeMultiViewStereoResults<>(new LookUpColorRgbFormats.SB_U8(), new MockLookUp());
    alg.processScenePoints(scene, (idx) -> (5 * idx + 10) + "", (idx, r, g, b) -> {
        // we can assume the first view is called first, but that's not strictly required to be correct
        int expected = idx == 0 ? 10 : 15;
        assertEquals(expected, r);
        assertEquals(expected, g);
        assertEquals(expected, b);
        count++;
    });
    assertEquals(2, count);
}

Example 10 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class VisOdomBundleAdjustment method copyResults.

/**
 * Copies results back on to the local data structures
 */
private void copyResults() {
    final SceneStructureMetric structure = bundle.getStructure();
    // skip the first frame since it's fixed
    for (int frameIdx = 1; frameIdx < frames.size; frameIdx++) {
        BFrame bf = frames.get(frameIdx);
        structure.getParentToView(frameIdx).invert(bf.frame_to_world);
    }
    int featureIdx = 0;
    for (int trackIdx = 0; trackIdx < tracks.size; trackIdx++) {
        BTrack bt = tracks.get(trackIdx);
        if (!bt.selected) {
            continue;
        }
        SceneStructureCommon.Point sp = structure.points.get(featureIdx);
        sp.get(bt.worldLoc);
        featureIdx++;
    }
}