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

Example 31 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations 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 32 with SceneObservations

use of boofcv.abst.geo.bundle.SceneObservations 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);
    }
}