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

Example 21 with SceneStructureMetric

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

the class TestSceneStructureMetric method getParentToView.

@Test
void getParentToView() {
    var scene = new SceneStructureMetric(false);
    scene.initialize(1, 3, 4);
    // add a motion to make the index off by one
    scene.addMotion(false, new Se3_F64());
    for (int i = 0; i < scene.views.size; i++) {
        scene.setView(i, -1, true, SpecialEuclideanOps_F64.eulerXyz(i, 0, 0, 0, 0, 0, null));
    }
    for (int i = 0; i < scene.views.size; i++) {
        assertEquals(i, scene.getParentToView(i).T.x, UtilEjml.TEST_F64);
    }
}

Example 22 with SceneStructureMetric

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

the class TestSceneStructureMetric method assignIDsToRigidPoints.

@Test
void assignIDsToRigidPoints() {
    var scene = new SceneStructureMetric(false);
    scene.initialize(1, 2, 2, 3, 2);
    scene.setRigid(0, false, new Se3_F64(), 2);
    scene.setRigid(1, true, new Se3_F64(), 3);
    scene.assignIDsToRigidPoints();
    for (int i = 0; i < 2; i++) {
        assertEquals(0, scene.lookupRigid[i]);
    }
    for (int i = 0; i < 3; i++) {
        assertEquals(1, scene.lookupRigid[i + 2]);
    }
    assertEquals(0, scene.rigids.data[0].indexFirst);
    assertEquals(2, scene.rigids.data[1].indexFirst);
}

Example 23 with SceneStructureMetric

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

the class CommonBundleAdjustmentMetricSchurJacobian method createSceneChainSameMotion.

/**
 * Create a scene where a "stereo" camera is created that moves. The right to left transform is fixed and common
 * across all views
 */
static SceneStructureMetric createSceneChainSameMotion(Random rand, boolean homogenous) {
    SceneStructureMetric out = new SceneStructureMetric(homogenous);
    int numSteps = 3;
    out.initialize(1, numSteps, 5);
    out.setCamera(0, true, new CameraPinhole(200, 300, 0.1, 400, 500, 1, 1));
    // Create a fixed transform for left to right camera
    int motionIdx = out.addMotion(false, SpecialEuclideanOps_F64.eulerXyz(0.25, 0.01, -0.05, 0.01, 0.02, -0.1, null));
    if (homogenous) {
        for (int i = 0; i < out.points.size; i++) {
            double w = rand.nextDouble() * 0.5 + 0.5;
            out.setPoint(i, w * (i + 1), w * (i + 2 * rand.nextGaussian()), w * (2 * i - 3 * rand.nextGaussian()), w);
        }
    } else {
        for (int i = 0; i < out.points.size; i++) {
            out.setPoint(i, i + 1, i + 2 * rand.nextGaussian(), 2 * i - 3 * rand.nextGaussian());
        }
    }
    for (int step = 0; step < numSteps; step++) {
        out.setView(step, -1, motionIdx, step - 1);
        out.connectViewToCamera(step, 0);
    }
    // Assign first point to all views then the other points to just one view
    for (int i = 0; i < out.views.size; i++) {
        out.points.data[0].views.add(i);
    }
    for (int i = 1; i < out.points.size; i++) {
        out.points.data[i].views.add((i - 1) % out.views.size);
    }
    // Sanity check
    assertEquals(1, out.motions.size);
    assertEquals(numSteps, out.views.size);
    return out;
}

Example 24 with SceneStructureMetric

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

the class CommonBundleAdjustmentMetricSchurJacobian method twoViewsOneMotion.

public void twoViewsOneMotion(boolean homogenous, boolean knownMotion) {
    SceneStructureMetric structure = createSceneStereo(rand, homogenous);
    SceneObservations observations = createObservations(rand, structure);
    structure.motions.forIdx((i, m) -> m.known = knownMotion);
    var param = new double[structure.getParameterCount()];
    new CodecSceneStructureMetric().encode(structure, param);
    var alg = createAlg();
    var jac = createJacobian(alg);
    var func = new BundleAdjustmentMetricResidualFunction();
    alg.configure(structure, observations);
    func.configure(structure, observations);
    // DerivativeChecker.jacobianPrint(func, jac, param, 100*UtilEjml.TEST_F64_SQ );
    assertTrue(DerivativeChecker.jacobian(func, jac, param, 100 * UtilEjml.TEST_F64_SQ));
}

Example 25 with SceneStructureMetric

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

the class TestCodecSceneStructureMetric method createSceneStereo.

/**
 * Create a scene where a "stereo" camera is created that moves. The right to left transform is fixed and common
 * across all views
 */
static SceneStructureMetric createSceneStereo(Random rand, boolean homogenous) {
    SceneStructureMetric out = new SceneStructureMetric(homogenous);
    int numSteps = 2;
    out.initialize(2, 2 * numSteps, 10);
    // Left camera
    out.setCamera(0, true, new CameraPinhole(200, 300, 0.1, 400, 500, 1, 1));
    // Right camera
    out.setCamera(1, false, new CameraPinhole(201 + rand.nextGaussian(), 200, 0.01, 401 + rand.nextGaussian(), 50 + rand.nextGaussian(), 1, 1));
    // Create a fixed transform for left to right camera
    int leftToRightIdx = out.addMotion(true, SpecialEuclideanOps_F64.eulerXyz(0.25, 0.01, -0.05, 0.01, 0.02, -0.1, null));
    if (homogenous) {
        for (int i = 0; i < out.points.size; i++) {
            double w = rand.nextDouble() * 0.5 + 0.5;
            out.setPoint(i, w * (i + 1), w * (i + 2 * rand.nextGaussian()), w * (2 * i - 3 * rand.nextGaussian()), w);
        }
    } else {
        for (int i = 0; i < out.points.size; i++) {
            out.setPoint(i, i + 1, i + 2 * rand.nextGaussian(), 2 * i - 3 * rand.nextGaussian());
        }
    }
    for (int step = 0; step < numSteps; step++) {
        Se3_F64 world_to_left = SpecialEuclideanOps_F64.eulerXyz(0.1, -0.15, -0.05 + step * 0.2, rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.05, null);
        out.setView(step * 2, 0, false, world_to_left);
        out.setView(step * 2 + 1, -1, leftToRightIdx, step * 2);
        out.connectViewToCamera(step * 2 + 1, 1);
    }
    // Assign first point to all views then the other points to just one view
    for (int i = 0; i < out.views.size; i++) {
        out.points.data[0].views.add(i);
    }
    for (int i = 1; i < out.points.size; i++) {
        out.points.data[i].views.add((i - 1) % out.views.size);
    }
    // Sanity check
    assertEquals(numSteps + 1, out.motions.size);
    assertEquals(numSteps * 2, out.views.size);
    return out;
}