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

Example 71 with DMatrixRMaj

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

the class TestSelfCalibrationLinearDualQuadratic method solveWithTrificalInput.

/**
 * Create a trifocal tensor, extract camera matrices, and see if it can find the solution
 */
@Test
void solveWithTrificalInput() {
    CameraPinhole intrinsic = new CameraPinhole(500, 500, 0, 0, 0, 0, 0);
    List<CameraPinhole> intrinsics = new ArrayList<>();
    for (int i = 0; i < 3; i++) {
        intrinsics.add(intrinsic);
    }
    renderGood(intrinsics);
    List<AssociatedTriple> obs = new ArrayList<>();
    for (int i = 0; i < cloud.size(); i++) {
        Point3D_F64 X = cloud.get(i);
        AssociatedTriple t = new AssociatedTriple();
        t.p1 = PerspectiveOps.renderPixel(listCameraToWorld.get(0), intrinsic, X, null);
        t.p2 = PerspectiveOps.renderPixel(listCameraToWorld.get(1), intrinsic, X, null);
        t.p3 = PerspectiveOps.renderPixel(listCameraToWorld.get(2), intrinsic, X, null);
        obs.add(t);
    }
    ConfigTrifocal config = new ConfigTrifocal();
    config.which = EnumTrifocal.LINEAR_7;
    Estimate1ofTrifocalTensor estimate = FactoryMultiView.trifocal_1(config);
    TrifocalTensor tensor = new TrifocalTensor();
    assertTrue(estimate.process(obs, tensor));
    DMatrixRMaj P1 = CommonOps_DDRM.identity(3, 4);
    DMatrixRMaj P2 = new DMatrixRMaj(3, 4);
    DMatrixRMaj P3 = new DMatrixRMaj(3, 4);
    MultiViewOps.trifocalToCameraMatrices(tensor, P2, P3);
    SelfCalibrationLinearDualQuadratic alg = new SelfCalibrationLinearDualQuadratic(1.0);
    alg.addCameraMatrix(P1);
    alg.addCameraMatrix(P2);
    alg.addCameraMatrix(P3);
    assertEquals(GeometricResult.SUCCESS, alg.solve());
    FastAccess<Intrinsic> found = alg.getIntrinsics();
    assertEquals(3, found.size());
    for (int i = 0; i < found.size(); i++) {
        Intrinsic f = found.get(i);
        assertEquals(500, f.fx, UtilEjml.TEST_F64_SQ);
        assertEquals(500, f.fy, UtilEjml.TEST_F64_SQ);
        assertEquals(0, f.skew, UtilEjml.TEST_F64_SQ);
    }
}

Example 72 with DMatrixRMaj

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

the class TestSelfCalibrationLinearDualQuadratic method constructMatrix.

@Test
void constructMatrix() {
    List<CameraPinhole> intrinsics = new ArrayList<>();
    for (int i = 0; i < 10; i++) {
        intrinsics.add(new CameraPinhole(400 + i * 5, 420, 0.1, 0, 0, 0, 0));
    }
    renderGood(intrinsics);
    SelfCalibrationLinearDualQuadratic alg = new SelfCalibrationLinearDualQuadratic(false);
    addProjectives(alg);
    DMatrixRMaj L = new DMatrixRMaj(3, 3);
    alg.constructMatrix(L);
    DMatrixRMaj q = new DMatrixRMaj(10, 1);
    q.data[0] = Q.get(0, 0);
    q.data[1] = Q.get(0, 1);
    q.data[2] = Q.get(0, 2);
    q.data[3] = Q.get(0, 3);
    q.data[4] = Q.get(1, 1);
    q.data[5] = Q.get(1, 2);
    q.data[6] = Q.get(1, 3);
    q.data[7] = Q.get(2, 2);
    q.data[8] = Q.get(2, 3);
    q.data[9] = Q.get(3, 3);
    // double[] sv = SingularOps_DDRM.singularValues(L);
    // for (int i = 0; i < sv.length; i++) {
    // System.out.println("sv["+i+"] = "+sv[i]);
    // }
    DMatrixRMaj found = new DMatrixRMaj(L.numRows, 1);
    // See if it's the null space
    CommonOps_DDRM.mult(L, q, found);
    assertEquals(0, NormOps_DDRM.normF(found), 10 * UtilEjml.TEST_F64);
}

Example 73 with DMatrixRMaj

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

the class TestSelfCalibrationLinearRotationSingle method performTest.

private void performTest(CameraPinhole intrinsic) {
    // compute planes at infinity
    List<Homography2D_F64> homographies = new ArrayList<>();
    for (int i = 0; i < listP.size(); i++) {
        DMatrixRMaj P = listP.get(i);
        Equation eq = new Equation();
        eq.alias(P, "P", planeAtInfinity, "p");
        eq.process("H = P(:,0:2) - P(:,3)*p'");
        Homography2D_F64 H = new Homography2D_F64();
        DConvertMatrixStruct.convert(eq.lookupDDRM("H"), H);
        homographies.add(H);
    }
    SelfCalibrationLinearRotationSingle alg = new SelfCalibrationLinearRotationSingle();
    CameraPinhole found = new CameraPinhole();
    assertTrue(alg.estimate(homographies, found));
    assertEquals(intrinsic.fx, found.fx, UtilEjml.TEST_F64_SQ);
    assertEquals(intrinsic.fy, found.fy, UtilEjml.TEST_F64_SQ);
    assertEquals(intrinsic.cx, found.cx, UtilEjml.TEST_F64_SQ);
    assertEquals(intrinsic.cy, found.cy, UtilEjml.TEST_F64_SQ);
    assertEquals(intrinsic.skew, found.skew, UtilEjml.TEST_F64_SQ);
}

Example 74 with DMatrixRMaj

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

the class TestSelfCalibrationLinearRotationSingle method checkFailure.

private void checkFailure() {
    // compute planes at infinity
    List<Homography2D_F64> homographies = new ArrayList<>();
    for (int i = 0; i < listP.size(); i++) {
        DMatrixRMaj P = listP.get(i);
        Equation eq = new Equation();
        eq.alias(P, "P", planeAtInfinity, "p");
        eq.process("H = P(:,0:2) - P(:,3)*p'");
        Homography2D_F64 H = new Homography2D_F64();
        DConvertMatrixStruct.convert(eq.lookupDDRM("H"), H);
        homographies.add(H);
    }
    SelfCalibrationLinearRotationSingle alg = new SelfCalibrationLinearRotationSingle();
    CameraPinhole found = new CameraPinhole();
    assertFalse(alg.estimate(homographies, found));
}

Example 75 with DMatrixRMaj

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

the class TestSelfCalibrationLinearRotationSingle method checkLinearSystem.

@Test
void checkLinearSystem() {
    CameraPinhole intrinsic = new CameraPinhole(400, 420, 0.1, 450, 475, 0, 0);
    renderRotationOnly(intrinsic);
    DMatrixRMaj K = new DMatrixRMaj(3, 3);
    PerspectiveOps.pinholeToMatrix(intrinsic, K);
    DMatrixRMaj w = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.multTransB(K, K, w);
    // compute planes at infinity
    List<Homography2D_F64> homographies = new ArrayList<>();
    for (int i = 0; i < listP.size(); i++) {
        // System.out.println("projective "+i);
        DMatrixRMaj P = listP.get(i);
        Equation eq = new Equation();
        eq.alias(P, "P", planeAtInfinity, "p", K, "K", w, "w");
        eq.process("H = P(:,0:2) - P(:,3)*p'");
        // eq.process("w2 = H*w*H'");
        // eq.process("w2 = w2 - w");
        // System.out.println("w");
        // eq.lookupDDRM("w").print();
        // System.out.println("w2");
        // eq.lookupDDRM("w2").print();
        Homography2D_F64 H = new Homography2D_F64();
        DConvertMatrixStruct.convert(eq.lookupDDRM("H"), H);
        homographies.add(H);
    // H.print();
    }
    SelfCalibrationLinearRotationSingle alg = new SelfCalibrationLinearRotationSingle();
    alg.ensureDeterminantOfOne(homographies);
    int N = homographies.size();
    DMatrixRMaj A = new DMatrixRMaj(6 * N, 6);
    DMatrixRMaj X = new DMatrixRMaj(6, 1);
    DMatrixRMaj found = new DMatrixRMaj(A.numRows, 1);
    for (int i = 0; i < homographies.size(); i++) {
        alg.add(i, homographies.get(i), A);
    }
    X.data[0] = w.data[0];
    X.data[1] = w.data[1];
    X.data[2] = w.data[2];
    X.data[3] = w.data[4];
    X.data[4] = w.data[5];
    X.data[5] = w.data[8];
    CommonOps_DDRM.mult(A, X, found);
    assertEquals(0, NormOps_DDRM.normF(found), UtilEjml.TEST_F64);
}