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

Example 1 with BundlePinholeBrown

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

the class TestBundleAdjustmentOps method convert_bundleBrown_brown.

@Test
void convert_bundleBrown_brown() {
    var src = new BundlePinholeBrown().setK(2, 3, 0, 7, 3).setRadial(1, 2).setTangential(-1, -9);
    var dst = new CameraPinholeBrown().fsetK(1, 2, 3, 4, 5, 6, 7).fsetRadial(-1, -2).fsetTangental(0.1, 0.2);
    assertSame(dst, BundleAdjustmentOps.convert(src, width, height, dst));
    assertArrayEquals(src.radial, dst.radial);
    assertEquals(src.t1, dst.t1);
    assertEquals(src.t2, dst.t2);
    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);
    assertEquals(width, dst.width);
    assertEquals(height, dst.height);
}

Example 2 with BundlePinholeBrown

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

the class CalibrateStereoPlanar method refineAll.

/**
 * Jointly refines both cameras together
 *
 * @param parameters (input) initial estimate and is updated if refine is successful
 */
private void refineAll(StereoParameters parameters) {
    Se3_F64 left_to_right = parameters.right_to_left.invert(null);
    final SceneStructureMetric structure = bundleUtils.getStructure();
    final SceneObservations observations = bundleUtils.getObservations();
    final SceneStructureMetric structureLeft = calibLeft.getStructure();
    final SceneStructureMetric structureRight = calibRight.getStructure();
    int numViews = structureLeft.views.size;
    // left and right cameras. n views, and 1 known calibration target
    structure.initialize(2, numViews * 2, numViews + 1, layout.size(), 1);
    // initialize the cameras
    structure.setCamera(0, false, structureLeft.cameras.get(0).model);
    structure.setCamera(1, false, structureRight.cameras.get(0).model);
    // configure the known calibration target
    structure.setRigid(0, true, new Se3_F64(), layout.size());
    SceneStructureMetric.Rigid rigid = structure.rigids.data[0];
    for (int i = 0; i < layout.size(); i++) {
        rigid.setPoint(i, layout.get(i).x, layout.get(i).y, 0);
    }
    // initialize the views. Right views will be relative to left and will share the same baseline
    int left_to_right_idx = structure.addMotion(false, left_to_right);
    for (int viewIndex = 0; viewIndex < numViews; viewIndex++) {
        int world_to_left_idx = structure.addMotion(false, structureLeft.motions.get(viewIndex).motion);
        structure.setView(viewIndex * 2, 0, world_to_left_idx, -1);
        structure.setView(viewIndex * 2 + 1, 1, left_to_right_idx, viewIndex * 2);
    }
    // Add observations for left and right camera
    observations.initialize(structure.views.size, true);
    for (int viewIndex = 0; viewIndex < numViews; viewIndex++) {
        SceneObservations.View oviewLeft = observations.getViewRigid(viewIndex * 2);
        CalibrationObservation left = calibLeft.observations.get(viewIndex);
        for (int j = 0; j < left.size(); j++) {
            PointIndex2D_F64 p = left.get(j);
            oviewLeft.add(p.index, (float) p.p.x, (float) p.p.y);
            structure.connectPointToView(p.index, viewIndex * 2);
        }
    }
    for (int viewIndex = 0; viewIndex < numViews; viewIndex++) {
        SceneObservations.View oviewRight = observations.getViewRigid(viewIndex * 2 + 1);
        CalibrationObservation right = calibRight.observations.get(viewIndex);
        for (int j = 0; j < right.size(); j++) {
            PointIndex2D_F64 p = right.get(j);
            oviewRight.add(p.index, (float) p.p.x, (float) p.p.y);
            structure.connectPointToView(p.index, viewIndex * 2 + 1);
        }
    }
    if (verbose != null)
        verbose.println("Joint bundle adjustment");
    if (!bundleUtils.process())
        return;
    // save the output
    structure.motions.get(left_to_right_idx).motion.invert(parameters.right_to_left);
    BundleAdjustmentOps.convert(((BundlePinholeBrown) structure.cameras.get(0).model), parameters.left.width, parameters.left.height, parameters.left);
    BundleAdjustmentOps.convert(((BundlePinholeBrown) structure.cameras.get(1).model), parameters.left.width, parameters.left.height, parameters.right);
}

Example 3 with BundlePinholeBrown

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

the class TestBundleAdjustmentOps method convert_brown_to_bundleBrown.

@Test
void convert_brown_to_bundleBrown() {
    var src = new CameraPinholeBrown().fsetK(1, 2, 3, 4, 5, 6, 7).fsetRadial(-1, -2).fsetTangental(0.1, 0.2);
    var dst = new BundlePinholeBrown(true, false);
    assertSame(dst, BundleAdjustmentOps.convert(src, dst));
    assertFalse(dst.zeroSkew);
    assertTrue(dst.tangential);
    assertArrayEquals(src.radial, dst.radial);
    assertEquals(src.t1, dst.t1);
    assertEquals(src.t2, dst.t2);
    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 BundlePinholeBrown

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

the class Zhang99CameraBrown method initializeCamera.

@Override
public BundleAdjustmentCamera initializeCamera(DMatrixRMaj K, List<DMatrixRMaj> homographies, List<CalibrationObservation> observations) {
    computeRadial.process(K, homographies, observations);
    BundlePinholeBrown cam = new BundlePinholeBrown(assumeZeroSkew, includeTangential);
    cam.radial = computeRadial.getParameters().clone();
    cam.setK(K);
    return cam;
}

Example 5 with BundlePinholeBrown

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