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

Example 16 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class BundleAdjustmentMetricSchurJacobian method declareStorageWorldToView.

/**
 * Declare storage and create a look up table for world to view for all relative views
 */
private void declareStorageWorldToView(SceneStructureMetric structure) {
    mapWorldToView.clear();
    storageSe3.reset();
    for (int viewIdx = 0; viewIdx < structure.views.size; viewIdx++) {
        SceneStructureMetric.View v = structure.views.get(viewIdx);
        if (v.parent == null)
            continue;
        Se3_F64 world_to_view = storageSe3.grow();
        mapWorldToView.put(v, world_to_view);
    }
}

Example 17 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class CalibrationPlanarGridZhang99 method convertIntoBundleStructure.

/**
 * Convert it into a data structure understood by {@link BundleAdjustment}
 */
public void convertIntoBundleStructure(List<Se3_F64> motions, DMatrixRMaj K, List<DMatrixRMaj> homographies, List<CalibrationObservation> calibrationObservations) {
    structure = new SceneStructureMetric(false);
    structure.initialize(1, motions.size(), -1, layout.size(), 1);
    observations = new SceneObservations();
    observations.initialize(motions.size(), true);
    // A single camera is assumed, that's what is being calibrated!
    structure.setCamera(0, false, cameraGenerator.initializeCamera(K, homographies, calibrationObservations));
    // A single rigid planar target is being viewed. It is assumed to be centered at the origin
    structure.setRigid(0, true, new Se3_F64(), layout.size());
    // Where the points are on the calibration target
    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);
    }
    // Add the initial estimate of each view's location and the points observed
    for (int viewIdx = 0; viewIdx < motions.size(); viewIdx++) {
        structure.setView(viewIdx, 0, false, motions.get(viewIdx));
        SceneObservations.View v = observations.getViewRigid(viewIdx);
        CalibrationObservation ca = calibrationObservations.get(viewIdx);
        for (int j = 0; j < ca.size(); j++) {
            PointIndex2D_F64 p = ca.get(j);
            v.add(p.index, (float) p.p.x, (float) p.p.y);
            structure.connectPointToView(p.index, viewIdx);
        }
    }
}

Example 18 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class TestMultiViewOps method triangulatePoints.

/**
 * Construct a scene with perfect observations. See if triangulation returns the same points
 */
@Test
void triangulatePoints() {
    CameraPinhole intrinsic = new CameraPinhole(500, 500, 0, 500, 500, 1000, 1000);
    List<Point3D_F64> points = UtilPoint3D_F64.random(new Point3D_F64(0, 0, 2), -0.5, 0.5, 100, rand);
    Se3_F64 view0_to_view1 = SpecialEuclideanOps_F64.eulerXyz(0.3, 0, 0, 0.1, -0.1, 0, null);
    SceneStructureMetric structure = new SceneStructureMetric(false);
    SceneObservations observations = new SceneObservations();
    observations.initialize(2);
    structure.initialize(1, 2, points.size());
    structure.setCamera(0, true, intrinsic);
    structure.setView(0, 0, true, new Se3_F64());
    structure.setView(1, 0, false, view0_to_view1);
    SceneObservations.View v0 = observations.getView(0);
    SceneObservations.View v1 = observations.getView(1);
    for (int i = 0; i < points.size(); i++) {
        Point3D_F64 X = points.get(i);
        Point2D_F64 p0 = PerspectiveOps.renderPixel(intrinsic, X, null);
        Point2D_F64 p1 = PerspectiveOps.renderPixel(view0_to_view1, intrinsic, X, null);
        v0.add(i, (float) p0.x, (float) p0.y);
        v1.add(i, (float) p1.x, (float) p1.y);
        structure.connectPointToView(i, 0);
        structure.connectPointToView(i, 1);
    }
    MultiViewOps.triangulatePoints(structure, observations);
    Point3D_F64 X = new Point3D_F64();
    for (int i = 0; i < points.size(); i++) {
        structure.getPoints().get(i).get(X);
        assertEquals(0, points.get(i).distance(X), UtilEjml.TEST_F64_SQ);
    }
}

Example 19 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class TestBundleAdjustmentMetricResidualFunction method changeInParamChangesOutput.

void changeInParamChangesOutput(boolean homogenous) {
    SceneStructureMetric structure = createScene(rand, homogenous, false, false);
    double[] param = new double[structure.getParameterCount()];
    new CodecSceneStructureMetric().encode(structure, param);
    // Create random observations
    SceneObservations obs = createObservations(rand, structure);
    BundleAdjustmentMetricResidualFunction alg = new BundleAdjustmentMetricResidualFunction();
    alg.configure(structure, obs);
    double[] original = new double[alg.getNumOfOutputsM()];
    double[] found = new double[alg.getNumOfOutputsM()];
    alg.process(param, original);
    for (int paramIndex = 0; paramIndex < original.length; paramIndex++) {
        double v = param[paramIndex];
        param[paramIndex] += 0.001;
        alg.process(param, found);
        boolean identical = true;
        for (int i = 0; i < found.length; i++) {
            if (Math.abs(original[i] - found[i]) > UtilEjml.TEST_F64) {
                identical = false;
                break;
            }
        }
        assertFalse(identical);
        param[paramIndex] = v;
    }
}

Example 20 with SceneStructureMetric

use of boofcv.abst.geo.bundle.SceneStructureMetric in project BoofCV by lessthanoptimal.

the class TestCodecSceneStructureMetric method encode_decode.

void encode_decode(boolean homogenous, boolean hasRigid) {
    SceneStructureMetric original = createScene(rand, homogenous, hasRigid, false);
    CodecSceneStructureMetric codec = new CodecSceneStructureMetric();
    int pointLength = homogenous ? 4 : 3;
    int N = original.getUnknownMotionCount() * 6 + original.getUnknownRigidCount() * 6 + original.points.size * pointLength + original.getUnknownCameraParameterCount();
    assertEquals(N, original.getParameterCount());
    double[] param = new double[N];
    codec.encode(original, param);
    SceneStructureMetric found = createScene(rand, homogenous, hasRigid, false);
    codec.decode(param, found);
    assertEquals(homogenous, found.isHomogenous());
    for (int i = 0; i < original.points.size; i++) {
        assertTrue(original.points.data[i].distance(found.points.data[i]) < UtilEjml.TEST_F64);
    }
    for (int i = 0; i < original.cameras.size; i++) {
        SceneStructureCommon.Camera o = original.cameras.data[i];
        SceneStructureCommon.Camera f = found.cameras.data[i];
        double[] po = new double[o.model.getIntrinsicCount()];
        double[] pf = new double[f.model.getIntrinsicCount()];
        o.model.getIntrinsic(po, 0);
        f.model.getIntrinsic(pf, 0);
        assertArrayEquals(po, pf, UtilEjml.TEST_F64);
    }
    for (int i = 0; i < original.motions.size; i++) {
        SceneStructureMetric.Motion o = original.motions.data[i];
        SceneStructureMetric.Motion f = found.motions.data[i];
        assertTrue(MatrixFeatures_DDRM.isIdentical(o.motion.R, f.motion.R, UtilEjml.TEST_F64));
        assertEquals(o.motion.T.x, f.motion.T.x, UtilEjml.TEST_F64);
        assertEquals(o.motion.T.y, f.motion.T.y, UtilEjml.TEST_F64);
        assertEquals(o.motion.T.z, f.motion.T.z, UtilEjml.TEST_F64);
    }
    for (int i = 0; i < original.rigids.size; i++) {
        SceneStructureMetric.Rigid o = original.rigids.data[i];
        SceneStructureMetric.Rigid f = found.rigids.data[i];
        assertTrue(MatrixFeatures_DDRM.isIdentical(o.object_to_world.R, f.object_to_world.R, UtilEjml.TEST_F64));
        assertEquals(o.object_to_world.T.x, f.object_to_world.T.x, UtilEjml.TEST_F64);
        assertEquals(o.object_to_world.T.y, f.object_to_world.T.y, UtilEjml.TEST_F64);
        assertEquals(o.object_to_world.T.z, f.object_to_world.T.z, UtilEjml.TEST_F64);
    }
}