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

Example 51 with SceneStructureMetric

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

the class TestGenerateStereoPairGraphFromScene method allTogether.

/**
 * Simple test where viewA and viewB only share very far away points and should have a low score. A and C
 * share some much closer points and should have a good score.
 */
@Test
void allTogether() {
    var viewToId = new TIntObjectHashMap<String>();
    var scene = new SceneStructureMetric(false);
    scene.initialize(3, 3, 10);
    for (int i = 0; i < 3; i++) {
        viewToId.put(i, "" + (i + 1));
        scene.setView(i, i, true, new Se3_F64());
        scene.setCamera(i, true, new CameraPinhole(200, 200, 0, 150, 150, 300, 300));
        scene.motions.get(i).motion.T.x = i;
    }
    // Create the points far away
    for (int i = 0; i < 5; i++) {
        scene.setPoint(i, 0, 0, 1e4);
        scene.points.get(i).views.setTo(0, 1);
    }
    // Add the up close points
    for (int i = 5; i < 10; i++) {
        scene.setPoint(i, 0, 0, 4);
        scene.points.get(i).views.setTo(0, 2);
    }
    var alg = new GenerateStereoPairGraphFromScene();
    alg.process(viewToId, scene);
    StereoPairGraph found = alg.getStereoGraph();
    assertEquals(3, found.vertexes.size());
    StereoPairGraph.Vertex vA = Objects.requireNonNull(found.vertexes.get("1"));
    StereoPairGraph.Vertex vB = Objects.requireNonNull(found.vertexes.get("2"));
    StereoPairGraph.Vertex vC = Objects.requireNonNull(found.vertexes.get("3"));
    assertEquals(0, vA.indexSba);
    assertEquals(1, vB.indexSba);
    assertEquals(2, vC.indexSba);
    assertEquals(2, vA.pairs.size());
    assertEquals(1, vB.pairs.size());
    assertEquals(1, vC.pairs.size());
    double quality_a_b = vA.pairs.get(0).quality3D;
    double quality_a_c = vA.pairs.get(1).quality3D;
    assertTrue(quality_a_c > quality_a_b * 2);
    assertTrue(quality_a_b > 0.0 && quality_a_b <= 1.0);
    assertTrue(quality_a_c > 0.0 && quality_a_c <= 1.0);
}

Example 52 with SceneStructureMetric

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

the class ThreeViewEstimateMetricScene method setupMetricBundleAdjustment.

/**
 * Using the initial metric reconstruction, provide the initial configurations for bundle adjustment
 */
private void setupMetricBundleAdjustment(List<AssociatedTriple> inliers) {
    // Construct bundle adjustment data structure
    structure = new SceneStructureMetric(false);
    structure.initialize(3, 3, inliers.size());
    observations = new SceneObservations();
    observations.initialize(3);
    for (int i = 0; i < listPinhole.size(); i++) {
        CameraPinhole cp = listPinhole.get(i);
        BundlePinholeSimplified bp = new BundlePinholeSimplified();
        bp.f = cp.fx;
        structure.setCamera(i, false, bp);
        structure.setView(i, i, i == 0, listWorldToView.get(i));
    }
    for (int i = 0; i < inliers.size(); i++) {
        AssociatedTriple t = inliers.get(i);
        observations.getView(0).add(i, (float) t.p1.x, (float) t.p1.y);
        observations.getView(1).add(i, (float) t.p2.x, (float) t.p2.y);
        observations.getView(2).add(i, (float) t.p3.x, (float) t.p3.y);
        structure.connectPointToView(i, 0);
        structure.connectPointToView(i, 1);
        structure.connectPointToView(i, 2);
    }
    // Initial estimate for point 3D locations
    triangulatePoints(structure, observations);
}

Example 53 with SceneStructureMetric

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

the class RefineMetricWorkingGraph method triangulateAndSave.

/**
 * Triangulates a new 3D feature, adds it to the structure, and links observations to it
 *
 * @param inlierIdx which inlier is being triangulated
 */
void triangulateAndSave(SceneWorkingGraph.InlierInfo inlierSet, int inlierIdx) {
    final SceneStructureMetric structure = metricSba.structure;
    final SceneObservations observations = metricSba.observations;
    final TriangulateNViewsMetricH triangulator = metricSba.triangulator;
    // Get a list of observations in normalized image coordinates
    for (int inlierViewIdx = 0; inlierViewIdx < sceneViewIntIds.size; inlierViewIdx++) {
        int viewID = sceneViewIntIds.get(inlierViewIdx);
        int obsIdx = inlierSet.observations.get(inlierViewIdx).get(inlierIdx);
        observations.getView(viewID).getPixel(obsIdx, pixelObserved);
        listPixelToNorm.get(viewID).compute(pixelObserved.x, pixelObserved.y, pixelNormalized.get(inlierViewIdx));
    }
    // a mistake
    if (!triangulator.triangulate(pixelNormalized.toList(), listPoses.toList(), found3D))
        return;
    // Verify it's not behind the camera. Unclear if it should give up here or try to optimize it
    // if( triangulated.z*triangulated.w < 0 ) {
    // if( verbose != null ) verbose.println("Triangulated point behind the camera");
    // return;
    // }
    // listPoses is relative to view0. Bring it into global reference frame
    SePointOps_F64.transform(view0_to_world, found3D, found3D);
    // Add the new 3D point to the scene
    int pointID = structure.points.size;
    SceneStructureCommon.Point point3D = structure.points.grow();
    if (structure.isHomogenous())
        point3D.set(found3D.x, found3D.y, found3D.z, found3D.w);
    else
        point3D.set(found3D.x / found3D.w, found3D.y / found3D.w, found3D.z / found3D.w);
    // Only assigned this 3D point to views which are unassigned.
    for (int i = 0; i < unassigned.size; i++) {
        int inlierViewIdx = unassigned.get(i);
        int obsIdx = inlierSet.observations.get(inlierViewIdx).get(inlierIdx);
        int viewID = sceneViewIntIds.get(inlierViewIdx);
        observations.getView(viewID).point.set(obsIdx, pointID);
        point3D.views.add(viewID);
    }
}