Examples with BundlePinholeSimplified boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified used on opensource projects

Example 11 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified 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 12 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class TestBundleToRectificationStereoParameters method processView2.

/**
 * Basic test to see if variables are initialized but doesn't check correctness.
 */
@Test
void processView2() {
    var alg = new BundleToRectificationStereoParameters();
    var bundle1 = new BundlePinholeSimplified(200, 0.1, -0.5);
    var bundle2 = new BundlePinholeSimplified(220, 0.1, -0.5);
    Se3_F64 view1_to_view2 = SpecialEuclideanOps_F64.eulerXyz(1.0, 0, -0.1, 0, 0, 0, null);
    // Easiest to initialize view-1 this way
    alg.setView1(bundle1, 100, 150);
    // Invoke the function being tested
    alg.processView2(bundle2, 100, 150, view1_to_view2);
    // See if everything is initialized
    assertTrue(CommonOps_DDRM.elementSum(alg.undist_to_rect1) != 0);
    assertTrue(CommonOps_DDRM.elementSum(alg.undist_to_rect2) != 0);
    assertTrue(CommonOps_FDRM.elementSum(alg.undist_to_rect1_F32) != 0);
    assertTrue(CommonOps_FDRM.elementSum(alg.undist_to_rect2_F32) != 0);
    assertFalse(MatrixFeatures_DDRM.isEquals(alg.undist_to_rect1, alg.undist_to_rect2, UtilEjml.TEST_F64));
    assertFalse(MatrixFeatures_FDRM.isEquals(alg.undist_to_rect1_F32, alg.undist_to_rect2_F32, UtilEjml.TEST_F32));
    assertTrue(CommonOps_DDRM.elementSum(alg.rectifiedK) != 0);
    assertTrue(CommonOps_DDRM.elementSum(alg.rotate_orig_to_rect) != 0);
    assertTrue(alg.rectifiedShape.height * alg.rectifiedShape.width > 0);
}

Example 13 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class SceneMergingOperations method loadExtrinsicsIntrinsics.

/**
 * Loads information about the view's intrinsics and estimated intrinsics in the specified scene
 *
 * @param scene Which scene is being considered
 * @param inliers Information on the views and inlier set used to estimate the target view
 * @param listWorldToViewSrc (Output) Extrinsics
 * @param listIntrinsicsSrc (Output) Intrinsics
 */
private void loadExtrinsicsIntrinsics(SceneWorkingGraph scene, SceneWorkingGraph.InlierInfo inliers, List<Se3_F64> listWorldToViewSrc, DogArray<RemoveBrownPtoN_F64> listIntrinsicsSrc) {
    // Clear lists
    listWorldToViewSrc.clear();
    listIntrinsicsSrc.reset();
    // Go through each view and extract it's SE3
    for (int viewIdx = 0; viewIdx < inliers.views.size; viewIdx++) {
        PairwiseImageGraph.View pview = inliers.views.get(viewIdx);
        SceneWorkingGraph.View wview = Objects.requireNonNull(scene.views.get(pview.id));
        BundlePinholeSimplified cam = scene.getViewCamera(wview).intrinsic;
        // Save the view's pose
        listWorldToViewSrc.add(wview.world_to_view);
        // Convert the intrinsics model to one that can be used to go from pixel to normalized
        RemoveBrownPtoN_F64 pixelToNorm = listIntrinsicsSrc.grow();
        pixelToNorm.setK(cam.f, cam.f, 0, 0, 0);
        pixelToNorm.setDistortion(cam.k1, cam.k2);
    }
}

Example 14 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified in project BoofCV by lessthanoptimal.

the class SceneMergingOperations method mergeStructure.

/**
 * Finds views which are in 'src' but not in 'dst' scene and copies them over while applying the extrinsic
 * transform. Keeps tracks of which views are in common too.
 *
 * @param src (Input) The source scene which which is merged into 'dst'
 * @param dst (Input/Output) The destination scene
 * @param src_to_dst (Input) Known transform from the coordinate system of src to dst.
 * @param scenesInEachView (Output) Modified to include changes to 'dst' in views that are in src and not dst
 */
void mergeStructure(SceneWorkingGraph src, SceneWorkingGraph dst, ScaleSe3_F64 src_to_dst, PairwiseViewScenes scenesInEachView) {
    mergedViews.clear();
    duplicateViews.clear();
    Se3_F64 src_to_view = new Se3_F64();
    // src_to_dst is between the world coordinate systems. We need dst_to_src * src_to_view
    Se3_F64 transform_dst_to_src = src_to_dst.transform.invert(null);
    for (int srcViewIdx = 0; srcViewIdx < src.listViews.size(); srcViewIdx++) {
        SceneWorkingGraph.View srcView = src.listViews.get(srcViewIdx);
        boolean copySrc = false;
        SceneWorkingGraph.View dstView;
        if (dst.views.containsKey(srcView.pview.id)) {
            // Both contain the same scene
            dstView = dst.views.get(srcView.pview.id);
            duplicateViews.add(dstView);
            if (verbose != null) {
                BundlePinholeSimplified srcIntrinsic = src.getViewCamera(srcView).intrinsic;
                BundlePinholeSimplified dstIntrinsic = dst.getViewCamera(dstView).intrinsic;
                verbose.printf("view='%s', sets={%d %d}, scores: %.1f vs %.1f, src.f=%.1f dst.f=%.1f\n", srcView.pview.id, srcView.inliers.size, dstView.inliers.size, srcView.getBestInlierScore(), dstView.getBestInlierScore(), srcIntrinsic.f, dstIntrinsic.f);
            }
        } else {
            // Need to add the dst to the list of scenes which contains this view. Do not mess with the counters
            // since that's handle by the toggle function
            // if (scenesInEachView.getView(srcView.pview).viewedBy.contains(dst.index))
            // throw new RuntimeException("BUG!");
            scenesInEachView.getView(srcView.pview).viewedBy.add(dst.index);
            scenesInEachView.getView(srcView.pview).viewedBy.sort(sorter);
            // NOTE: This sorted insert could be speed up
            // If the camera doesn't exist in 'dst' add a new camera. Otherwise, keep 'dst' version of it
            // unmodified.
            SceneWorkingGraph.Camera cameraSrc = src.getViewCamera(srcView);
            SceneWorkingGraph.Camera cameraDst = dst.cameras.get(cameraSrc.indexDB);
            if (cameraDst == null) {
                cameraDst = dst.addCameraCopy(cameraSrc);
            }
            dstView = dst.addView(srcView.pview, cameraDst);
            copySrc = true;
            if (verbose != null) {
                BundlePinholeSimplified srcIntrinsic = src.getViewCamera(srcView).intrinsic;
                verbose.printf("view='%s', sets=%d, score: %.1f, src.f=%.1f\n", srcView.pview.id, srcView.inliers.size, srcView.getBestInlierScore(), srcIntrinsic.f);
            }
        }
        mergedViews.add(dstView);
        // Always copy the src inliers into the dst. This ensures islands do not form
        for (int infoIdx = 0; infoIdx < srcView.inliers.size; infoIdx++) {
            // It's not uncommon to have the same views in both inlier sets. Ignoring this potential inefficiency
            // for now since it works
            dstView.inliers.grow().setTo(srcView.inliers.get(infoIdx));
        }
        // Stop here the view already exists in dst
        if (!copySrc)
            continue;
        src_to_view.setTo(srcView.world_to_view);
        src_to_view.T.scale(src_to_dst.scale);
        transform_dst_to_src.concat(src_to_view, dstView.world_to_view);
    }
}

Example 15 with BundlePinholeSimplified

use of boofcv.alg.geo.bundle.cameras.BundlePinholeSimplified 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);
}