Examples with TriangulateNViewsMetricH boofcv.abst.geo.TriangulateNViewsMetricH used on opensource projects

Example 1 with TriangulateNViewsMetricH

use of boofcv.abst.geo.TriangulateNViewsMetricH in project BoofCV by lessthanoptimal.

the class EstimateViewUtils method configureSbaStructure.

/**
 * Configures data structures for running SBA. Which observations are used is specified by the provided inliers.
 * By default all cameras and views are set to known. If these need to be optimized for a specific use case then
 * 'known' should be set to false.
 *
 * @param inliersThreeView Specifies the observations
 */
public void configureSbaStructure(List<AssociatedTriple> inliersThreeView) {
    final SceneStructureMetric structure = metricSba.structure;
    final SceneObservations observations = metricSba.observations;
    // Even if the cameras are all the same, we will tell that they are different just because the bookkeeping
    // is so much easier and results are the same
    structure.initialize(3, 3, usedThreeViewInliers.size);
    observations.initialize(3);
    // All cameras are known
    structure.setCamera(0, true, camera1);
    structure.setCamera(1, true, camera2);
    structure.setCamera(2, true, camera3);
    // All transforms are known but the target
    structure.setView(0, 0, true, view1_to_view1);
    structure.setView(1, 1, true, view1_to_view2);
    structure.setView(2, 2, true, view1_to_target);
    observations.getView(0).resize(usedThreeViewInliers.size());
    observations.getView(1).resize(usedThreeViewInliers.size());
    observations.getView(2).resize(usedThreeViewInliers.size());
    SceneObservations.View viewObs1 = observations.getView(0);
    SceneObservations.View viewObs2 = observations.getView(1);
    SceneObservations.View viewObs3 = observations.getView(2);
    final TriangulateNViewsMetricH triangulator = metricSba.triangulator;
    var foundX = new Point4D_F64();
    // Only use features that were in the inlier set for PnP
    for (int inlierCnt = 0; inlierCnt < usedThreeViewInliers.size(); inlierCnt++) {
        int threeViewInlierIndex = usedThreeViewInliers.get(inlierCnt);
        AssociatedTriple a = inliersThreeView.get(threeViewInlierIndex);
        // Pass in pixel observations for each view
        viewObs1.set(inlierCnt, inlierCnt, (float) a.p1.x, (float) a.p1.y);
        viewObs2.set(inlierCnt, inlierCnt, (float) a.p2.x, (float) a.p2.y);
        viewObs3.set(inlierCnt, inlierCnt, (float) a.p3.x, (float) a.p3.y);
        normalize1.compute(a.p1.x, a.p1.y, pixelNorms.get(0));
        normalize2.compute(a.p2.x, a.p2.y, pixelNorms.get(1));
        normalize3.compute(a.p3.x, a.p3.y, pixelNorms.get(2));
        if (!triangulator.triangulate(pixelNorms, listMotion, foundX)) {
            throw new RuntimeException("Triangulation failed. Possibly bad input. Handle this problem");
        }
        if (structure.isHomogenous())
            structure.setPoint(inlierCnt, foundX.x, foundX.y, foundX.z, foundX.w);
        else
            structure.setPoint(inlierCnt, foundX.x / foundX.w, foundX.y / foundX.w, foundX.z / foundX.w);
        structure.connectPointToView(inlierCnt, 0);
        structure.connectPointToView(inlierCnt, 1);
        structure.connectPointToView(inlierCnt, 2);
    }
}

Example 2 with TriangulateNViewsMetricH

use of boofcv.abst.geo.TriangulateNViewsMetricH in project BoofCV by lessthanoptimal.

the class MultiViewOps method triangulatePoints.

/**
 * Convenience function for initializing bundle adjustment parameters. Triangulates points using camera
 * position and pixel observations.
 *
 * @param structure camera locations
 * @param observations observations of features in the images
 */
public static void triangulatePoints(SceneStructureMetric structure, SceneObservations observations) {
    TriangulateNViewsMetricH triangulator = FactoryMultiView.triangulateNViewMetricH(ConfigTriangulation.GEOMETRIC());
    List<RemoveBrownPtoN_F64> list_p_to_n = new ArrayList<>();
    for (int i = 0; i < structure.cameras.size; i++) {
        RemoveBrownPtoN_F64 p2n = new RemoveBrownPtoN_F64();
        BundleAdjustmentCamera baseModel = Objects.requireNonNull(structure.cameras.data[i].model);
        if (baseModel instanceof BundlePinholeSimplified) {
            BundlePinholeSimplified cam = (BundlePinholeSimplified) baseModel;
            p2n.setK(cam.f, cam.f, 0, 0, 0).setDistortion(new double[] { cam.k1, cam.k2 }, 0, 0);
        } else if (baseModel instanceof BundlePinhole) {
            BundlePinhole cam = (BundlePinhole) baseModel;
            p2n.setK(cam.fx, cam.fy, cam.skew, cam.cx, cam.cy).setDistortion(new double[] { 0, 0 }, 0, 0);
        } else if (baseModel instanceof BundlePinholeBrown) {
            BundlePinholeBrown cam = (BundlePinholeBrown) baseModel;
            p2n.setK(cam.fx, cam.fy, cam.skew, cam.cx, cam.cy).setDistortion(cam.radial, cam.t1, cam.t2);
        } else {
            throw new RuntimeException("Unknown camera model!");
        }
        list_p_to_n.add(p2n);
    }
    DogArray<Point2D_F64> normObs = new DogArray<>(Point2D_F64::new);
    normObs.resize(3);
    final boolean homogenous = structure.isHomogenous();
    Point4D_F64 X = new Point4D_F64();
    List<Se3_F64> worldToViews = new ArrayList<>();
    for (int i = 0; i < structure.points.size; i++) {
        normObs.reset();
        worldToViews.clear();
        SceneStructureCommon.Point sp = structure.points.get(i);
        for (int j = 0; j < sp.views.size; j++) {
            int viewIdx = sp.views.get(j);
            SceneStructureMetric.View v = structure.views.data[viewIdx];
            worldToViews.add(structure.getParentToView(v));
            // get the observation in pixels
            Point2D_F64 n = normObs.grow();
            int pointidx = observations.views.get(viewIdx).point.indexOf(i);
            observations.views.get(viewIdx).getPixel(pointidx, n);
            // convert to normalized image coordinates
            list_p_to_n.get(v.camera).compute(n.x, n.y, n);
        }
        if (!triangulator.triangulate(normObs.toList(), worldToViews, X)) {
            // this should work unless the input is bad
            throw new RuntimeException("Triangulation failed. Bad input?");
        }
        if (homogenous)
            sp.set(X.x, X.y, X.z, X.w);
        else
            sp.set(X.x / X.w, X.y / X.w, X.z / X.w);
    }
}

Example 3 with TriangulateNViewsMetricH

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