Examples with DMatrix2x2 org.ejml.data.DMatrix2x2 used on opensource projects

Example 1 with DMatrix2x2

use of org.ejml.data.DMatrix2x2 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 2 with DMatrix2x2

use of org.ejml.data.DMatrix2x2 in project BoofCV by lessthanoptimal.

the class TestPnPInfinitesimalPlanePoseEstimation method constructR.

@Test
void constructR() {
    Equation eq = new Equation();
    eq.process("R_v=randn(3,3)");
    eq.process("R22=[1 2;3 4]");
    eq.process("c=[1.1;2.1]");
    eq.process("a=3.1");
    eq.process("b=[-0.4;0.2]");
    eq.process("R1=R_v*[R22 c;b' a]");
    eq.process("R2=R_v*[R22, -c;-b', a]");
    DMatrixRMaj found = new DMatrixRMaj(3, 3);
    DMatrixRMaj R_v = eq.lookupDDRM("R_v");
    DMatrix2x2 R22 = new DMatrix2x2();
    DConvertMatrixStruct.convert(eq.lookupDDRM("R22"), R22);
    Vector3D_F64 ca = new Vector3D_F64(1.1, 2.1, 3.1);
    PnPInfinitesimalPlanePoseEstimation.constructR(found, R_v, R22, -0.4, 0.2, ca, 1, new DMatrixRMaj(3, 3));
    assertTrue(MatrixFeatures_DDRM.isIdentical(eq.lookupDDRM("R1"), found, UtilEjml.TEST_F64));
    PnPInfinitesimalPlanePoseEstimation.constructR(found, R_v, R22, -0.4, 0.2, ca, -1, new DMatrixRMaj(3, 3));
    assertTrue(MatrixFeatures_DDRM.isIdentical(eq.lookupDDRM("R2"), found, UtilEjml.TEST_F64));
}

Example 3 with DMatrix2x2

use of org.ejml.data.DMatrix2x2 in project BoofCV by lessthanoptimal.

the class TestPnPInfinitesimalPlanePoseEstimation method largestSingularValue2x2.

@Test
void largestSingularValue2x2() {
    DMatrix2x2 M = new DMatrix2x2(1, -1.5, 0.5, 1.8);
    SimpleMatrix A = new SimpleMatrix(new double[][] { { M.a11, M.a12 }, { M.a21, M.a22 } });
    double[] s = A.svd().getSingularValues();
    PnPInfinitesimalPlanePoseEstimation alg = new PnPInfinitesimalPlanePoseEstimation();
    double found = alg.largestSingularValue2x2(M);
    assertEquals(s[0], found, UtilEjml.TEST_F64);
}

Example 4 with DMatrix2x2

use of org.ejml.data.DMatrix2x2 in project BoofCV by lessthanoptimal.

the class TestPnPInfinitesimalPlanePoseEstimation method compute_B.

@Test
void compute_B() {
    Equation eq = new Equation();
    eq.process("v=[1.1,0.5]'");
    eq.process("R_v=[1,2,3;4,5,6;7,8,9]'");
    eq.process("B=[eye(2),-v]*R_v");
    DMatrixRMaj v = eq.lookupDDRM("v");
    DMatrixRMaj R_v = eq.lookupDDRM("R_v");
    DMatrixRMaj expected = eq.lookupDDRM("B");
    double v1 = v.get(0);
    double v2 = v.get(1);
    DMatrix2x2 B = new DMatrix2x2();
    PnPInfinitesimalPlanePoseEstimation.compute_B(B, R_v, v1, v2);
    assertEquals(expected.get(0, 0), B.a11, UtilEjml.TEST_F64);
    assertEquals(expected.get(0, 1), B.a12, UtilEjml.TEST_F64);
    assertEquals(expected.get(1, 0), B.a21, UtilEjml.TEST_F64);
    assertEquals(expected.get(1, 1), B.a22, UtilEjml.TEST_F64);
}

Example 5 with DMatrix2x2

use of org.ejml.data.DMatrix2x2 in project BoofCV by lessthanoptimal.

the class HomographyTotalLeastSquares method computePseudo.

/**
 * Computes inv(A<sup>T</sup>*A)*A<sup>T</sup>
 */
static void computePseudo(DMatrixRMaj A, DMatrixRMaj output) {
    final int N = A.numRows;
    DMatrix2x2 m = new DMatrix2x2();
    DMatrix2x2 m_inv = new DMatrix2x2();
    for (int i = 0, index = 0; i < N; i++) {
        double a_i1 = A.data[index++];
        double a_i2 = A.data[index++];
        m.a11 += a_i1 * a_i1;
        m.a12 += a_i1 * a_i2;
        m.a22 += a_i2 * a_i2;
    }
    m.a21 = m.a12;
    CommonOps_DDF2.invert(m, m_inv);
    output.reshape(2, N);
    for (int i = 0, index = 0; i < N; i++) {
        output.data[i] = A.data[index++] * m_inv.a11 + A.data[index++] * m_inv.a12;
    }
    int end = 2 * N;
    for (int i = N, A_index = 0; i < end; i++) {
        output.data[i] = A.data[A_index++] * m_inv.a21 + A.data[A_index++] * m_inv.a22;
    }
}