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

Example 1 with BundlePinhole

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

the class GenericBundleAdjustmentMetricChecks method checkReprojectionError.

public static void checkReprojectionError(SceneStructureMetric structure, SceneObservations observations, double tol) {
    BundlePinhole c = (BundlePinhole) structure.cameras.get(0).model;
    // making a bunch of assumptions here...
    CameraPinhole intrinsic = new CameraPinhole(c.fx, c.fy, c.skew, c.cx, c.cy, 600, 600);
    WorldToCameraToPixel wcp = new WorldToCameraToPixel();
    PointIndex2D_F64 o = new PointIndex2D_F64();
    Point2D_F64 predicted = new Point2D_F64();
    if (structure.homogenous) {
        Point4D_F64 p4 = new Point4D_F64();
        Point3D_F64 p3 = new Point3D_F64();
        for (int viewIndex = 0; viewIndex < observations.views.size; viewIndex++) {
            SceneObservations.View v = observations.views.data[viewIndex];
            Se3_F64 parent_to_view = structure.getParentToView(viewIndex);
            wcp.configure(intrinsic, parent_to_view);
            for (int j = 0; j < v.point.size; j++) {
                v.getPixel(j, o);
                structure.points.data[o.index].get(p4);
                p3.x = p4.x / p4.w;
                p3.y = p4.y / p4.w;
                p3.z = p4.z / p4.w;
                wcp.transform(p3, predicted);
                double residual = o.p.distance(predicted);
                if (Math.abs(residual) > tol)
                    fail("Error is too large. " + residual);
            }
        }
    } else {
        Point3D_F64 p3 = new Point3D_F64();
        for (int viewIndex = 0; viewIndex < observations.views.size; viewIndex++) {
            SceneObservations.View v = observations.views.data[viewIndex];
            Se3_F64 parent_to_view = structure.getParentToView(viewIndex);
            wcp.configure(intrinsic, parent_to_view);
            for (int j = 0; j < v.point.size; j++) {
                v.getPixel(j, o);
                structure.points.data[o.index].get(p3);
                wcp.transform(p3, predicted);
                double residual = o.p.distance(predicted);
                if (Math.abs(residual) > tol)
                    fail("Error is too large. " + residual);
            }
        }
    }
}

Example 2 with BundlePinhole

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

the class TestPoint2Transform2BundleCamera method compare.

@Test
void compare() {
    var camera = new BundlePinhole();
    camera.setK(100, 100, 0, 100, 100);
    var alg = new Point2Transform2BundleCamera();
    alg.setModel(camera);
    var expected = new Point2D_F64();
    var found = new Point2D_F64();
    PerspectiveOps.renderPixel(new CameraPinhole(100, 100, 0, 100, 100, 0, 0), new Point3D_F64(0.1, -0.05, 1.0), expected);
    alg.compute(0.1, -0.05, found);
    assertEquals(0.0, expected.distance(found), UtilEjml.TEST_F64);
}

Example 3 with BundlePinhole

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

the class TestBundleAdjustmentOps method convert_brown_to_bundlePinhole.

@Test
void convert_brown_to_bundlePinhole() {
    var src = new CameraPinholeBrown().fsetK(1, 2, 3, 4, 5, 6, 7).fsetRadial(-1, -2).fsetTangental(0.1, 0.2);
    var dst = new BundlePinhole(true);
    assertSame(dst, BundleAdjustmentOps.convert(src, dst));
    assertFalse(dst.zeroSkew);
    assertEquals(src.fx, dst.fx);
    assertEquals(src.fy, dst.fy);
    assertEquals(src.cx, dst.cx);
    assertEquals(src.cy, dst.cy);
    assertEquals(src.skew, dst.skew);
}

Example 4 with BundlePinhole

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

the class TestBundleAdjustmentOps method convert_pinhole_to_bundlePinhole.

@Test
void convert_pinhole_to_bundlePinhole() {
    var src = new CameraPinhole(10, 12, 1, 1, 2, 10, 32);
    var dst = new BundlePinhole().setK(9, 3, 0, 0, 2);
    assertSame(dst, BundleAdjustmentOps.convert(src, dst));
    assertEquals(src.fx, dst.fx);
    assertEquals(src.fy, dst.fy);
    assertEquals(src.cx, dst.cx);
    assertEquals(src.cy, dst.cy);
    assertEquals(src.skew, dst.skew);
    assertFalse(dst.zeroSkew);
}

Example 5 with BundlePinhole

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