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

Example 1 with DMatrix3x3

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

the class PnPInfinitesimalPlanePoseEstimation method estimateTranslation.

/**
 * Estimate's the translation given the previously found rotation
 * @param R Rotation matrix
 * @param T (Output) estimated translation
 */
void estimateTranslation(DMatrixRMaj R, List<AssociatedPair> points, Vector3D_F64 T) {
    final int N = points.size();
    W.reshape(N, 3);
    y.reshape(N, 1);
    Wty.reshape(3, 1);
    DMatrix3x3 Rtmp = new DMatrix3x3();
    ConvertDMatrixStruct.convert(R, Rtmp);
    int indexY = 0, indexW = 0;
    for (int i = 0; i < N; i++) {
        AssociatedPair p = points.get(i);
        // rotate into camera frame
        double u1 = Rtmp.a11 * p.p1.x + Rtmp.a12 * p.p1.y;
        double u2 = Rtmp.a21 * p.p1.x + Rtmp.a22 * p.p1.y;
        double u3 = Rtmp.a31 * p.p1.x + Rtmp.a32 * p.p1.y;
        W.data[indexW++] = 1;
        W.data[indexW++] = 0;
        W.data[indexW++] = -p.p2.x;
        W.data[indexW++] = 0;
        W.data[indexW++] = 1;
        W.data[indexW++] = -p.p2.y;
        y.data[indexY++] = p.p2.x * u3 - u1;
        y.data[indexY++] = p.p2.y * u3 - u2;
    }
    // ======= Compute Pseudo Inverse
    // WW = inv(W^T*W)
    CommonOps_DDRM.multTransA(W, W, WW);
    CommonOps_DDRM.invert(WW);
    // W^T*y
    CommonOps_DDRM.multTransA(W, y, Wty);
    // translation = inv(W^T*W)*W^T*y
    W.reshape(N, 1);
    CommonOps_DDRM.mult(WW, Wty, W);
    T.x = W.data[0];
    T.y = W.data[1];
    T.z = W.data[2];
}