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

Example 1 with FunctionNtoMxN

use of org.ddogleg.optimization.functions.FunctionNtoMxN 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)));
}