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

Example 11 with DMatrixRMaj

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

the class BenchmarkStabilityFundamental method evaluateMinimal.

public void evaluateMinimal(GeoModelEstimatorN<DMatrixRMaj, AssociatedPair> estimatorN) {
    DistanceEpipolarConstraint distance = new DistanceEpipolarConstraint();
    Estimate1ofEpipolar estimator = new EstimateNto1ofEpipolar(estimatorN, distance, 1);
    scores = new ArrayList<>();
    int failed = 0;
    int numSamples = estimator.getMinimumPoints();
    Random rand = new Random(234);
    DMatrixRMaj F = new DMatrixRMaj(3, 3);
    for (int i = 0; i < 50; i++) {
        List<AssociatedPair> pairs = new ArrayList<>();
        // create a unique set of pairs
        while (pairs.size() < numSamples) {
            AssociatedPair p = observations.get(rand.nextInt(observations.size()));
            if (!pairs.contains(p)) {
                pairs.add(p);
            }
        }
        if (!estimator.process(pairs, F)) {
            failed++;
            continue;
        }
        // normalize the scale of F
        CommonOps_DDRM.scale(1.0 / CommonOps_DDRM.elementMaxAbs(F), F);
        double totalScore = 0;
        // score against all observations
        for (AssociatedPair p : observations) {
            double score = Math.abs(GeometryMath_F64.innerProd(p.p2, F, p.p1));
            if (Double.isNaN(score))
                System.out.println("Score is NaN");
            scores.add(score);
            totalScore += score;
        }
    // System.out.println("  score["+i+"] = "+totalScore);
    }
    Collections.sort(scores);
    System.out.printf(" Failures %3d  Score:  50%% = %6.3e  95%% = %6.3e\n", failed, scores.get(scores.size() / 2), scores.get((int) (scores.size() * 0.95)));
}

Example 12 with DMatrixRMaj

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

the class BenchmarkStabilityFundamental method evaluateAll.

public void evaluateAll(GeoModelEstimator1<DMatrixRMaj, AssociatedPair> estimator) {
    scores = new ArrayList<>();
    int failed = 0;
    DMatrixRMaj F = new DMatrixRMaj(3, 3);
    for (int i = 0; i < 50; i++) {
        if (!estimator.process(observations, F)) {
            failed++;
            continue;
        }
        // normalize the scale of F
        CommonOps_DDRM.scale(1.0 / CommonOps_DDRM.elementMaxAbs(F), F);
        // score against all observations
        for (AssociatedPair p : observations) {
            double score = Math.abs(GeometryMath_F64.innerProd(p.p2, F, p.p1));
            if (Double.isNaN(score))
                System.out.println("Score is NaN");
            scores.add(score);
        }
    }
    Collections.sort(scores);
    System.out.printf(" Failures %3d  Score:  50%% = %6.3e  95%% = %6.3e\n", failed, scores.get(scores.size() / 2), scores.get((int) (scores.size() * 0.95)));
}

Example 13 with DMatrixRMaj

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

the class MultiViewOps method decomposeCameraMatrix.

/**
 * <p>
 * Decomposes a camera matrix P=A*[R|T], where A is an upper triangular camera calibration
 * matrix, R is a rotation matrix, and T is a translation vector.
 *
 * <ul>
 * <li> NOTE: There are multiple valid solutions to this problem and only one solution is returned.
 * <li> NOTE: The camera center will be on the plane at infinity.
 * </ul>
 * </p>
 *
 * @param P Input: Camera matrix, 3 by 4
 * @param K Output: Camera calibration matrix, 3 by 3.
 * @param pose Output: The rotation and translation.
 */
public static void decomposeCameraMatrix(DMatrixRMaj P, DMatrixRMaj K, Se3_F64 pose) {
    DMatrixRMaj KR = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.extract(P, 0, 3, 0, 3, KR, 0, 0);
    QRDecomposition<DMatrixRMaj> qr = DecompositionFactory_DDRM.qr(3, 3);
    if (!CommonOps_DDRM.invert(KR))
        throw new RuntimeException("Inverse failed!  Bad input?");
    if (!qr.decompose(KR))
        throw new RuntimeException("QR decomposition failed!  Bad input?");
    DMatrixRMaj U = qr.getQ(null, false);
    DMatrixRMaj B = qr.getR(null, false);
    if (!CommonOps_DDRM.invert(U, pose.getR()))
        throw new RuntimeException("Inverse failed!  Bad input?");
    Point3D_F64 KT = new Point3D_F64(P.get(0, 3), P.get(1, 3), P.get(2, 3));
    GeometryMath_F64.mult(B, KT, pose.getT());
    if (!CommonOps_DDRM.invert(B, K))
        throw new RuntimeException("Inverse failed!  Bad input?");
    CommonOps_DDRM.scale(1.0 / K.get(2, 2), K);
}

Example 14 with DMatrixRMaj

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

the class MultiViewOps method extractCameraMatrices.

/**
 * <p>
 * Extract the camera matrices up to a common projective transform.
 * </p>
 *
 * <p>
 * NOTE: The camera matrix for the first view is assumed to be P1 = [I|0].
 * </p>
 *
 * @param tensor Trifocal tensor.  Not modified.
 * @param P2 Output: 3x4 camera matrix for views 1 to 2. Modified.
 * @param P3 Output: 3x4 camera matrix for views 1 to 3. Modified.
 */
public static void extractCameraMatrices(TrifocalTensor tensor, DMatrixRMaj P2, DMatrixRMaj P3) {
    // extract the epipoles
    Point3D_F64 e2 = new Point3D_F64();
    Point3D_F64 e3 = new Point3D_F64();
    extractEpipoles(tensor, e2, e3);
    // storage for intermediate results
    Point3D_F64 temp0 = new Point3D_F64();
    Point3D_F64 column = new Point3D_F64();
    // temp1 = [e3*e3^T -I]
    DMatrixRMaj temp1 = new DMatrixRMaj(3, 3);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            temp1.set(i, j, e3.getIndex(i) * e3.getIndex(j));
        }
        temp1.set(i, i, temp1.get(i, i) - 1);
    }
    // compute the camera matrices one column at a time
    for (int i = 0; i < 3; i++) {
        DMatrixRMaj T = tensor.getT(i);
        GeometryMath_F64.mult(T, e3, column);
        P2.set(0, i, column.x);
        P2.set(1, i, column.y);
        P2.set(2, i, column.z);
        P2.set(i, 3, e2.getIndex(i));
        GeometryMath_F64.multTran(T, e2, temp0);
        GeometryMath_F64.mult(temp1, temp0, column);
        P3.set(0, i, column.x);
        P3.set(1, i, column.y);
        P3.set(2, i, column.z);
        P3.set(i, 3, e3.getIndex(i));
    }
}

Example 15 with DMatrixRMaj

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

the class MultiViewOps method canonicalCamera.

/**
 * <p>
 * Given a fundamental matrix a pair of projection matrices [R|T] can be extracted.  There are multiple
 * solutions which can be found, the canonical projection matrix is defined as: <br>
 * <pre>
 * P=[I|0] and P'= [M|-M*t] = [[e']*F + e'*v^t | lambda*e']
 * </pre>
 * where e' is the epipole F<sup>T</sup>e' = 0, [e'] is the cross product matrix for the enclosed vector,
 * v is an arbitrary 3-vector and lambda is a non-zero scalar.
 * </p>
 *
 * <p>
 * Page 256 in R. Hartley, and A. Zisserman, "Multiple View Geometry in Computer Vision", 2nd Ed, Cambridge 2003
 * </p>
 *
 * @see #extractEpipoles
 *
 * @param F A fundamental matrix
 * @param v Arbitrary 3-vector.  Just pick some value, say (1,1,1).
 * @param lambda A non zero scalar.  Try one.
 * @param e2 Left epipole of fundamental matrix, F<sup>T</sup>*e2 = 0.
 * @return The canonical camera matrix P'
 */
public static DMatrixRMaj canonicalCamera(DMatrixRMaj F, Point3D_F64 e2, Vector3D_F64 v, double lambda) {
    DMatrixRMaj crossMatrix = new DMatrixRMaj(3, 3);
    GeometryMath_F64.crossMatrix(e2, crossMatrix);
    DMatrixRMaj outer = new DMatrixRMaj(3, 3);
    GeometryMath_F64.outerProd(e2, v, outer);
    DMatrixRMaj KR = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.mult(crossMatrix, F, KR);
    CommonOps_DDRM.add(KR, outer, KR);
    DMatrixRMaj P = new DMatrixRMaj(3, 4);
    CommonOps_DDRM.insert(KR, P, 0, 0);
    P.set(0, 3, lambda * e2.x);
    P.set(1, 3, lambda * e2.y);
    P.set(2, 3, lambda * e2.z);
    return P;
}