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

Example 56 with DMatrixRMaj

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

the class ProjectiveStructureFromHomographies method filterPointsOnPlaneAtInfinity.

/**
 * Identifies points which lie on the plane at infinity. That is done by computing x' = H*x and seeing if the w
 * term is nearly zero, e.g. x' = (x,y,w)
 */
void filterPointsOnPlaneAtInfinity(List<DMatrixRMaj> homographies_view1_to_view0, List<List<PointIndex2D_F64>> observations, int totalFeatures) {
    filtered.clear();
    for (int viewIdx = 0; viewIdx < homographies_view1_to_view0.size(); viewIdx++) {
        List<PointIndex2D_F64> filter = new ArrayList<>();
        filtered.add(filter);
        DMatrixRMaj H = homographies_view1_to_view0.get(viewIdx);
        List<PointIndex2D_F64> obs = observations.get(viewIdx);
        for (int i = 0; i < obs.size(); i++) {
            PointIndex2D_F64 p = obs.get(i);
            if (p.index < 0 || p.index >= totalFeatures)
                throw new IllegalArgumentException("Feature index outside of bounds. Must be from 0 to " + (totalFeatures - 1));
            GeometryMath_F64.mult(H, p.p, tmp);
            // Homogenous coordinates are scale invariant. A scale
            // needs to be picked for consistency. I picked the largest x or y value
            double m = Math.max(Math.abs(tmp.x), Math.abs(tmp.y));
            if (m == 0)
                m = 1;
            tmp.z /= m;
            // See if it's zero or almost zero, meaning it's on the plane at infinity
            if (Math.abs(tmp.z) > infinityThreshold) {
                filter.add(p);
            }
        }
    }
}

Example 57 with DMatrixRMaj

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

the class ResidualsTriangulateProjective method process.

@Override
public void process(double[] input, double[] output) {
    point.x = input[0];
    point.y = input[1];
    point.z = input[2];
    point.w = input[3];
    int outputIdx = 0;
    for (int i = 0; i < observations.size(); i++) {
        Point2D_F64 p = observations.get(i);
        DMatrixRMaj m = cameraMatrices.get(i);
        PerspectiveOps.renderPixel(m, point, predicted);
        output[outputIdx++] = predicted.x - p.x;
        output[outputIdx++] = predicted.y - p.y;
    }
}

Example 58 with DMatrixRMaj

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

the class TrifocalExtractGeometries method extractFundmental.

/**
 * <p>
 * Extract the fundamental matrices between views 1 + 2 and views 1 + 3. The returned Fundamental
 * matrices will have the following properties: x<sub>i</sub><sup>T</sup>*Fi*x<sub>1</sub> = 0, where i is view 2 or 3.
 * </p>
 *
 * <p>
 * NOTE: The first camera is assumed to have the camera matrix of P1 = [I|0]. Thus observations in pixels for
 * the first camera will not meet the epipolar constraint when applied to the returned fundamental matrices.
 * </p>
 *
 * <pre>
 * F21=[e2]x *[T1,T2,T3]*e3 and F31 = [e3]x*[T1,T2,T3]*e3
 * </pre>
 *
 * @param F21 (Output) Fundamental matrix
 * @param F31 (Output) Fundamental matrix
 */
public void extractFundmental(DMatrixRMaj F21, DMatrixRMaj F31) {
    // 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, temp0);
        GeometryMath_F64.cross(e2, temp0, column);
        F21.set(0, i, column.x);
        F21.set(1, i, column.y);
        F21.set(2, i, column.z);
        GeometryMath_F64.multTran(T, e2, temp0);
        GeometryMath_F64.cross(e3, temp0, column);
        F31.set(0, i, column.x);
        F31.set(1, i, column.y);
        F31.set(2, i, column.z);
    }
}

Example 59 with DMatrixRMaj

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

the class TrifocalExtractGeometries method extractCamera.

/**
 * <p>
 * Extract the camera matrices up to a common projective transform.
 * </p>
 *
 * <p>P2 = [[T1.T2.T3]e3|e2] and P3=[(e3*e2<sup>T</sup>-I)[T1',T2',T3'|e2|e3]</p>
 *
 * <p>
 * NOTE: The camera matrix for the first view is assumed to be P1 = [I|0].
 * </p>
 *
 * @param P2 Output: 3x4 camera matrix for views 2. Modified.
 * @param P3 Output: 3x4 camera matrix for views 3. Modified.
 */
public void extractCamera(DMatrixRMaj P2, DMatrixRMaj P3) {
    // temp1 = [e3*e3^T -I]
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            temp1.set(i, j, e3.getIdx(i) * e3.getIdx(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.getIdx(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.getIdx(i));
    }
}

Example 60 with DMatrixRMaj

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

the class TrifocalLinearPoint7 method removeNormalization.

/**
 * Translates the trifocal tensor back into regular coordinate system
 */
protected void removeNormalization(TrifocalTensor solution) {
    DMatrixRMaj N2_inv = N2.matrixInv(null);
    DMatrixRMaj N3_inv = N3.matrixInv(null);
    DMatrixRMaj N1 = this.N1.matrix(null);
    for (int i = 0; i < 3; i++) {
        DMatrixRMaj T = solution.getT(i);
        for (int j = 0; j < 3; j++) {
            for (int k = 0; k < 3; k++) {
                double sum = 0;
                for (int r = 0; r < 3; r++) {
                    double n1 = N1.get(r, i);
                    DMatrixRMaj TN = solutionN.getT(r);
                    for (int s = 0; s < 3; s++) {
                        double n2 = N2_inv.get(j, s);
                        for (int t = 0; t < 3; t++) {
                            sum += n1 * n2 * N3_inv.get(k, t) * TN.get(s, t);
                        }
                    }
                }
                T.set(j, k, sum);
            }
        }
    }
}