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

Example 1 with DMatrix3

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

the class SelfCalibrationLinearDualQuadratic method constructMatrix.

/**
 * Constructs the linear system by applying specified constraints
 */
void constructMatrix(DMatrixRMaj L) {
    L.reshape(cameras.size * eqs, 10);
    // Known aspect ratio constraint makes more sense as a square
    double RR = this.aspectRatio * this.aspectRatio;
    // F = P*Q*P'
    // F is an arbitrary variable name and not fundamental matrix
    int index = 0;
    for (int i = 0; i < cameras.size; i++) {
        Projective P = cameras.get(i);
        DMatrix3x3 A = P.A;
        DMatrix3 B = P.a;
        // hard to tell if this helped or hurt. Keeping commented out for future investigation on proper scaling
        // double scale = NormOps_DDF3.normF(P.A);
        // CommonOps_DDF3.divide(P.A,scale);
        // CommonOps_DDF3.divide(P.a,scale);
        // Scaling camera matrices P on input by [1/f 0 0; 0 1/f 0; 0 0 1] seems to make it slightly worse on
        // real world data!
        // constraint for a zero principle point
        // row for F[0,2] == 0
        L.data[index++] = A.a11 * A.a31;
        L.data[index++] = (A.a12 * A.a31 + A.a11 * A.a32);
        L.data[index++] = (A.a13 * A.a31 + A.a11 * A.a33);
        L.data[index++] = (B.a1 * A.a31 + A.a11 * B.a3);
        L.data[index++] = A.a12 * A.a32;
        L.data[index++] = (A.a13 * A.a32 + A.a12 * A.a33);
        L.data[index++] = (B.a1 * A.a32 + A.a12 * B.a3);
        L.data[index++] = A.a13 * A.a33;
        L.data[index++] = (B.a1 * A.a33 + A.a13 * B.a3);
        L.data[index++] = B.a1 * B.a3;
        // row for F[1,2] == 0
        L.data[index++] = A.a21 * A.a31;
        L.data[index++] = (A.a22 * A.a31 + A.a21 * A.a32);
        L.data[index++] = (A.a23 * A.a31 + A.a21 * A.a33);
        L.data[index++] = (B.a2 * A.a31 + A.a21 * B.a3);
        L.data[index++] = A.a22 * A.a32;
        L.data[index++] = (A.a23 * A.a32 + A.a22 * A.a33);
        L.data[index++] = (B.a2 * A.a32 + A.a22 * B.a3);
        L.data[index++] = A.a23 * A.a33;
        L.data[index++] = (B.a2 * A.a33 + A.a23 * B.a3);
        L.data[index++] = B.a2 * B.a3;
        if (zeroSkew) {
            // row for F[0,1] == 0
            L.data[index++] = A.a11 * A.a21;
            L.data[index++] = (A.a12 * A.a21 + A.a11 * A.a22);
            L.data[index++] = (A.a13 * A.a21 + A.a11 * A.a23);
            L.data[index++] = (B.a1 * A.a21 + A.a11 * B.a2);
            L.data[index++] = A.a12 * A.a22;
            L.data[index++] = (A.a13 * A.a22 + A.a12 * A.a23);
            L.data[index++] = (B.a1 * A.a22 + A.a12 * B.a2);
            L.data[index++] = A.a13 * A.a23;
            L.data[index++] = (B.a1 * A.a23 + A.a13 * B.a2);
            L.data[index++] = B.a1 * B.a2;
        }
        if (knownAspect) {
            // aspect^2*F[0,0]-F[1,1]
            L.data[index++] = A.a11 * A.a11 * RR - A.a21 * A.a21;
            L.data[index++] = 2 * (A.a11 * A.a12 * RR - A.a21 * A.a22);
            L.data[index++] = 2 * (A.a11 * A.a13 * RR - A.a21 * A.a23);
            L.data[index++] = 2 * (A.a11 * B.a1 * RR - A.a21 * B.a2);
            L.data[index++] = A.a12 * A.a12 * RR - A.a22 * A.a22;
            L.data[index++] = 2 * (A.a12 * A.a13 * RR - A.a22 * A.a23);
            L.data[index++] = 2 * (A.a12 * B.a1 * RR - A.a22 * B.a2);
            L.data[index++] = A.a13 * A.a13 * RR - A.a23 * A.a23;
            L.data[index++] = 2 * (A.a13 * B.a1 * RR - A.a23 * B.a2);
            L.data[index++] = B.a1 * B.a1 * RR - B.a2 * B.a2;
        }
    }
}

Example 2 with DMatrix3

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

the class TestMultiViewOps method decomposeAbsDualQuadratic.

@Test
void decomposeAbsDualQuadratic() {
    Equation eq = new Equation();
    eq.process("K=[300 1 50;0 310 60; 0 0 1]");
    eq.process("w=K*K'");
    eq.process("p=rand(3,1)");
    eq.process("u=-p'*K");
    eq.process("H=[K [0;0;0]; u 1]");
    eq.process("Q=H*diag([1 1 1 0])*H'");
    DMatrixRMaj Q = eq.lookupDDRM("Q");
    DMatrix4x4 Q_in = new DMatrix4x4();
    DConvertMatrixStruct.convert(Q, Q_in);
    CommonOps_DDF4.scale(0.0394, Q_in);
    DMatrix3x3 w = new DMatrix3x3();
    DMatrix3 p = new DMatrix3();
    assertTrue(MultiViewOps.decomposeAbsDualQuadratic(Q_in, w, p));
    assertTrue(MatrixFeatures_D.isIdentical(eq.lookupDDRM("w"), w, UtilEjml.TEST_F64));
    assertTrue(MatrixFeatures_D.isIdentical(eq.lookupDDRM("p"), p, UtilEjml.TEST_F64));
}

Example 3 with DMatrix3

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

the class TestEstimatePlaneAtInfinityGivenK method computeRotation.

@Test
void computeRotation() {
    DMatrix3 t = new DMatrix3(2, 0.5, 1.2);
    DMatrix3x3 R = new DMatrix3x3();
    EstimatePlaneAtInfinityGivenK.computeRotation(t, R);
    double n = NormOps_DDF3.normF(t);
    assertEquals(n, t.a1, UtilEjml.TEST_F64);
    assertEquals(0, t.a2, UtilEjml.TEST_F64);
    assertEquals(0, t.a3, UtilEjml.TEST_F64);
    DMatrix3 ta = new DMatrix3(2, 0.5, 1.2);
    CommonOps_DDF3.mult(R, ta, t);
    assertEquals(n, t.a1, UtilEjml.TEST_F64);
    assertEquals(0, t.a2, UtilEjml.TEST_F64);
    assertEquals(0, t.a3, UtilEjml.TEST_F64);
}

Example 4 with DMatrix3

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

the class ProjectiveToMetricCameraDualQuadratic method refineCamerasAlgebraic.

/**
 * This refines intrinsic parameters by minimizing algebraic error. If anything goes wrong it doesn't update
 * the intrinsics. Also updates the rectifying homography.
 */
void refineCamerasAlgebraic(List<ElevateViewInfo> views, List<DMatrixRMaj> cameraMatrices, int numCameras) {
    // Refiner can't handle non-zero skew yet
    if (!selfCalib.zeroSkew) {
        if (verbose != null)
            verbose.println("Skipping refine since skew is not zero");
        return;
    }
    // Just skip everything if it has been turned off
    if (refiner.converge.maxIterations <= 0)
        return;
    // Sanity check the P0 is implicit
    BoofMiscOps.checkEq(views.size(), cameraMatrices.size() + 1);
    // Make sure refiner applies the same constraints that the linear estimator applies
    refiner.knownAspect = selfCalib.isKnownAspect();
    refiner.knownPrinciplePoint = true;
    // Configure the refiner. If multiple views use the same camera this constraint is applied
    refiner.initialize(numCameras, views.size());
    DMatrix3 planeAtInfinity = selfCalib.getPlaneAtInfinity();
    refiner.setPlaneAtInfinity(planeAtInfinity.a1, planeAtInfinity.a2, planeAtInfinity.a3);
    for (int cameraIdx = 0; cameraIdx < numCameras; cameraIdx++) {
        CameraPinhole merged = workCameras.get(cameraIdx);
        refiner.setCamera(cameraIdx, merged.fx, merged.cx, merged.cy, merged.fy / merged.fx);
    }
    for (int viewIdx = 0; viewIdx < views.size(); viewIdx++) {
        if (viewIdx == 0)
            refiner.setProjective(0, P1);
        else
            refiner.setProjective(viewIdx, cameraMatrices.get(viewIdx - 1));
        refiner.setViewToCamera(viewIdx, views.get(viewIdx).cameraID);
    }
    // Refine and change nothing if it fails
    if (!refiner.refine()) {
        if (verbose != null)
            verbose.println("Refine failed! Ignoring results");
        return;
    }
    if (verbose != null) {
        for (int i = 0; i < numCameras; i++) {
            CameraState refined = refiner.getCameras().get(i);
            verbose.printf("refined[%d] fx=%.1f fy=%.1f\n", i, refined.fx, refined.fx * refined.aspectRatio);
        }
    }
    // Save the refined intrinsic parameters
    for (int i = 0; i < views.size(); i++) {
        ElevateViewInfo info = views.get(i);
        CameraState refined = refiner.getCameras().get(info.cameraID);
        CameraPinhole estimated = workCameras.get(info.cameraID);
        estimated.fx = refined.fx;
        estimated.fy = refined.aspectRatio * refined.fx;
    // refiner doesn't support non-zero skew yet
    }
    // Update rectifying homography using the new parameters
    // NOTE: This formulation of H requires P1=[I|0] which is true in this case
    PerspectiveOps.pinholeToMatrix(workCameras.get(views.get(0).cameraID), K);
    MultiViewOps.canonicalRectifyingHomographyFromKPinf(K, refiner.planeAtInfinity, H);
}