Examples with FunctionNtoM org.ddogleg.optimization.functions.FunctionNtoM used on opensource projects

Example 1 with FunctionNtoM

use of org.ddogleg.optimization.functions.FunctionNtoM in project BoofCV by lessthanoptimal.

the class ResidualTriangulateChecks method perfect.

/**
 * Give it perfect parameters and no noise in observations then try introducing some errors
 */
@Test
public void perfect() {
    createScene();
    FunctionNtoM alg = createAlg(obsPts, motionWorldToCamera);
    double[] input = new double[] { worldPoint.x, worldPoint.y, worldPoint.z };
    double[] output = new double[alg.getNumOfOutputsM()];
    alg.process(input, output);
    // there should be no errors
    double error = computeCost(output);
    assertEquals(0, error, 1e-8);
    // corrupt the parameter, which should cause errors in the residuals
    input[0] += 1;
    alg.process(input, output);
    error = computeCost(output);
    assertTrue(error > 0.1);
}

Example 2 with FunctionNtoM

use of org.ddogleg.optimization.functions.FunctionNtoM in project BoofCV by lessthanoptimal.

the class TestKannalaBrandtPtoS_F64 method jacobianOfDistorted.

/**
 * Compare to numerical Jacobian
 */
@Test
void jacobianOfDistorted() {
    CameraKannalaBrandt model = new CameraKannalaBrandt().fsetK(500, 550, 0.0, 600, 650);
    model.fsetSymmetric(1.0, 0.4).fsetRadial(1.1, 0.2, -0.01).fsetTangent(0.5, -0.1, 0.06, 0.12).fsetRadialTrig(0.01, 0.03, -0.03, 0.04).fsetTangentTrig(0.01, 0.2, 0.1, 0.4);
    FunctionNtoM function = new FunctionNtoM() {

        @Override
        public void process(/**/
        double[] input, /**/
        double[] output) {
            double theta = (double) input[0];
            double psi = (double) input[1];
            double r = (double) polynomial(model.symmetric, theta);
            double cospsi = Math.cos(psi);
            double sinpsi = Math.sin(psi);
            // distortion terms. radial and tangential
            double dr = (double) (polynomial(model.radial, theta) * polytrig(model.radialTrig, cospsi, sinpsi));
            double dt = (double) (polynomial(model.tangent, theta) * polytrig(model.tangentTrig, cospsi, sinpsi));
            // put it all together to get normalized image coordinates
            output[0] = (r + dr) * cospsi - dt * sinpsi;
            output[1] = (r + dr) * sinpsi + dt * cospsi;
        }

        @Override
        public int getNumOfInputsN() {
            return 2;
        }

        @Override
        public int getNumOfOutputsM() {
            return 2;
        }
    };
    var kb = new KannalaBrandtPtoS_F64(model);
    FunctionNtoMxN<DMatrixRMaj> jacobian = new FunctionNtoMxN<>() {

        final DMatrix2x2 a = new DMatrix2x2();

        @Override
        public int getNumOfInputsN() {
            return 2;
        }

        @Override
        public int getNumOfOutputsM() {
            return 2;
        }

        @Override
        public DMatrixRMaj declareMatrixMxN() {
            return new DMatrixRMaj(2, 2);
        }

        @Override
        public void process(/**/
        double[] input, DMatrixRMaj output) {
            double theta = (double) input[0];
            double psi = (double) input[1];
            double cospsi = Math.cos(psi);
            double sinpsi = Math.sin(psi);
            kb.jacobianOfDistorted(theta, cospsi, sinpsi, a);
            BoofMiscOps.convertMatrix(a, output);
        }
    };
    // DerivativeChecker.jacobianPrint(function, jacobian, new double[]{0.2, -0.4}, 1e-4);
    assertTrue(DerivativeChecker.jacobian(function, jacobian, new /**/
    double[] { 0.2, -0.4 }, UtilEjml.TEST_F64_SQ, Math.sqrt(UtilEjml.EPS)));
}

Example 3 with FunctionNtoM

use of org.ddogleg.optimization.functions.FunctionNtoM in project BoofCV by lessthanoptimal.

the class ResidualTriangulateProjectiveChecks method perfect.

/**
 * Give it perfect parameters and no noise in observations then try introducing some errors
 */
@Test
void perfect() {
    createScene();
    FunctionNtoM alg = createAlg(obsPixels, cameraMatrices);
    double[] input = new double[] { worldPoint.x, worldPoint.y, worldPoint.z, 1 };
    double[] output = new double[alg.getNumOfOutputsM()];
    alg.process(input, output);
    // there should be no errors
    double error = computeCost(output);
    assertEquals(0, error, 1e-8);
    // corrupt the parameter, which should cause errors in the residuals
    input[0] += 1;
    alg.process(input, output);
    error = computeCost(output);
    assertTrue(error > 0.1);
}

Example 4 with FunctionNtoM

use of org.ddogleg.optimization.functions.FunctionNtoM in project BoofCV by lessthanoptimal.

the class ResidualTriangulateMetricChecks method perfect.

/**
 * Give it perfect parameters and no noise in observations then try introducing some errors
 */
@Test
void perfect() {
    createScene();
    FunctionNtoM alg = createAlg(obsNorm, motionWorldToCamera);
    double[] input = new double[] { worldPoint.x, worldPoint.y, worldPoint.z };
    double[] output = new double[alg.getNumOfOutputsM()];
    alg.process(input, output);
    // there should be no errors
    double error = computeCost(output);
    assertEquals(0, error, 1e-8);
    // corrupt the parameter, which should cause errors in the residuals
    input[0] += 1;
    alg.process(input, output);
    error = computeCost(output);
    assertTrue(error > 0.1);
}