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

Example 36 with SceneStructureMetric

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

the class TestCodecSceneStructureMetric method createScene.

static SceneStructureMetric createScene(Random rand, boolean homogenous, boolean hasRigid, boolean hasRelative) {
    SceneStructureMetric out = new SceneStructureMetric(homogenous);
    int numRigid = hasRigid ? 2 : 0;
    out.initialize(2, 4, 4, 5, numRigid);
    out.setCamera(0, true, new CameraPinhole(200, 300, 0.1, 400, 500, 1, 1));
    out.setCamera(1, false, new CameraPinhole(201 + rand.nextGaussian(), 200, 0.01, 401 + rand.nextGaussian(), 50 + rand.nextGaussian(), 1, 1));
    if (hasRigid) {
        Se3_F64 worldToRigid0 = SpecialEuclideanOps_F64.eulerXyz(rand.nextGaussian() * 0.1, 0, 0.15, rand.nextGaussian() * 0.01 - 0.1, -0.01, 0.2, null);
        Se3_F64 worldToRigid1 = SpecialEuclideanOps_F64.eulerXyz(-0.1, -0.1, -0.3, -0.1, 0.3, 0, null);
        out.setRigid(0, false, worldToRigid0, 3);
        out.setRigid(1, true, worldToRigid1, 2);
        for (int i = 0; i < out.rigids.size; i++) {
            SceneStructureMetric.Rigid r = out.rigids.data[i];
            if (homogenous) {
                for (int j = 0; j < r.points.length; j++) {
                    double w = rand.nextDouble() * 3 + 0.5;
                    r.setPoint(j, rand.nextGaussian() * 0.2, rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.2, w);
                }
            } else {
                for (int j = 0; j < r.points.length; j++) {
                    r.setPoint(j, rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.2, rand.nextGaussian() * 0.1);
                }
            }
        }
        // assign All of first rigid's points to all views
        SceneStructureMetric.Rigid r = out.rigids.data[0];
        for (int idxPoint = 0; idxPoint < r.points.length; idxPoint++) {
            for (int i = 0; i < 4; i++) {
                r.points[idxPoint].views.add(i);
            }
        }
        // just the first point to each view after this
        r = out.rigids.data[1];
        for (int i = 0; i < 4; i++) {
            r.points[0].views.add(i);
        }
    }
    out.assignIDsToRigidPoints();
    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 i = 0; i < out.views.size; i++) {
        boolean fixed = i % 2 == 0;
        Se3_F64 a = new Se3_F64();
        if (fixed) {
            ConvertRotation3D_F64.eulerToMatrix(EulerType.YXY, 0.2 * i + 0.1, 0.7, 0, a.R);
            a.T.setTo(2, 3, i * 7.3 + 5);
        } else {
            ConvertRotation3D_F64.eulerToMatrix(EulerType.YXY, 0.2 * i + 0.1, rand.nextGaussian() * 0.1, 0, a.R);
            a.T.setTo(rand.nextGaussian() * 0.2, 3 * rand.nextGaussian() * 0.2, i * 7.3 + 5);
        }
        out.setView(i, i / 2, fixed, a);
        // Create a chain of relative views
        if (hasRelative)
            out.views.data[i].parent = i > 0 ? out.views.data[i - 1] : null;
    }
    // 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);
    }
    return out;
}

Example 37 with SceneStructureMetric

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

the class CommonBundleAdjustmentMetricSchurJacobian method compareToNumerical_relative.

public void compareToNumerical_relative(boolean homogenous) {
    SceneStructureMetric structure = createScene(rand, homogenous, false, true);
    SceneObservations observations = createObservations(rand, structure);
    // Try different patterns of relative views and attempt to cover all the edge cases
    compareToNumerical_relative(new boolean[] { true, true, true, false }, structure, observations);
    compareToNumerical_relative(new boolean[] { true, true, false, true }, structure, observations);
    compareToNumerical_relative(new boolean[] { true, true, false, false }, structure, observations);
    compareToNumerical_relative(new boolean[] { true, false, false, false }, structure, observations);
    compareToNumerical_relative(new boolean[] { true, false, true, false }, structure, observations);
    compareToNumerical_relative(new boolean[] { false, true, true, true }, structure, observations);
}

Example 38 with SceneStructureMetric

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

the class CommonBundleAdjustmentMetricSchurJacobian method compareToNumerical.

public void compareToNumerical(BundleAdjustmentMetricSchurJacobian<M> alg, boolean homogenous, boolean hasRigid, boolean hasRelative) {
    SceneStructureMetric structure = createScene(rand, homogenous, hasRigid, hasRelative);
    SceneObservations observations = createObservations(rand, structure);
    var param = new double[structure.getParameterCount()];
    new CodecSceneStructureMetric().encode(structure, param);
    var jac = createJacobian(alg);
    var func = new BundleAdjustmentMetricResidualFunction();
    alg.configure(structure, observations);
    func.configure(structure, observations);
    // TODO I think the Rodrigues jacobian is computed in a numerically unstable way.
    // multiplying tolerance by 100 is ridiculous
    // DerivativeChecker.jacobianPrint(func, jac, param, 100*UtilEjml.TEST_F64_SQ );
    assertTrue(DerivativeChecker.jacobian(func, jac, param, 110 * UtilEjml.TEST_F64_SQ));
}

Example 39 with SceneStructureMetric

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

the class CommonBundleAdjustmentMetricSchurJacobian method sameMotionInChain.

public void sameMotionInChain(boolean homogenous) {
    SceneStructureMetric structure = createSceneChainSameMotion(rand, homogenous);
    SceneObservations observations = createObservations(rand, structure);
    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 40 with SceneStructureMetric

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

the class TestBundleAdjustmentMetricResidualFunction method chainedRelativeViews.

void chainedRelativeViews(boolean homogenous) {
    SceneStructureMetric structure = createScene(rand, homogenous, false, false);
    // Make each view be relative to the previous view
    structure.views.forIdx((i, v) -> v.parent = i > 0 ? structure.views.data[i - 1] : null);
    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);
    // Now change view[1]. This should change residuals in 1,2,3 but not 0
    structure.getParentToView(1).T.x += 0.2;
    structure.getParentToView(1).T.y += 0.2;
    new CodecSceneStructureMetric().encode(structure, param);
    alg.process(param, found);
    // values should not be the same after this index
    int changeAfterIndex = obs.getView(0).size() * 2;
    for (int i = 0; i < found.length; i++) {
        if (i < changeAfterIndex) {
            assertEquals(original[i], found[i], UtilEjml.TEST_F64);
        } else {
            assertNotEquals(original[i], found[i], UtilEjml.TEST_F64);
        }
    }
}