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

Example 16 with DMatrixRMaj

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

the class MultiViewOps method extractFundamental.

/**
 * <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>
 *
 * @param tensor Trifocal tensor.  Not modified.
 * @param F2 Output: Fundamental matrix for views 1 and 2. Modified.
 * @param F3 Output: Fundamental matrix for views 1 and 3. Modified.
 */
public static void extractFundamental(TrifocalTensor tensor, DMatrixRMaj F2, DMatrixRMaj F3) {
    // 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();
    // compute the Fundamental 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);
        F2.set(0, i, column.x);
        F2.set(1, i, column.y);
        F2.set(2, i, column.z);
        GeometryMath_F64.multTran(T, e2, temp0);
        GeometryMath_F64.cross(e3, temp0, column);
        F3.set(0, i, column.x);
        F3.set(1, i, column.y);
        F3.set(2, i, column.z);
    }
}

Example 17 with DMatrixRMaj

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

the class CalibPoseAndPointRodriguesCodec method encode.

@Override
public void encode(CalibratedPoseAndPoint model, double[] param) {
    int paramIndex = 0;
    // first decode the transformation
    for (int i = 0; i < numViews; i++) {
        // don't encode if it is already known
        if (knownView[i])
            continue;
        Se3_F64 se = model.getWorldToCamera(i);
        // otherwise Rodrigues will have issues with the noise
        if (!svd.decompose(se.getR()))
            throw new RuntimeException("SVD failed");
        DMatrixRMaj U = svd.getU(null, false);
        DMatrixRMaj V = svd.getV(null, false);
        CommonOps_DDRM.multTransB(U, V, R);
        // extract Rodrigues coordinates
        ConvertRotation3D_F64.matrixToRodrigues(R, rotation);
        param[paramIndex++] = rotation.unitAxisRotation.x * rotation.theta;
        param[paramIndex++] = rotation.unitAxisRotation.y * rotation.theta;
        param[paramIndex++] = rotation.unitAxisRotation.z * rotation.theta;
        Vector3D_F64 T = se.getT();
        param[paramIndex++] = T.x;
        param[paramIndex++] = T.y;
        param[paramIndex++] = T.z;
    }
    for (int i = 0; i < numPoints; i++) {
        Point3D_F64 p = model.getPoint(i);
        param[paramIndex++] = p.x;
        param[paramIndex++] = p.y;
        param[paramIndex++] = p.z;
    }
}

Example 18 with DMatrixRMaj

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

the class FundamentalLinear7 method computeSolutions.

/**
 * <p>
 * Find the polynomial roots and for each root compute the Fundamental matrix.
 * Given the two matrices it will compute an alpha such that the determinant is zero.<br>
 *
 * det(&alpha*F1 + (1-&alpha;)*F2 ) = 0
 * </p>
 */
public void computeSolutions(FastQueue<DMatrixRMaj> solutions) {
    if (!rootFinder.process(poly))
        return;
    List<Complex_F64> zeros = rootFinder.getRoots();
    for (Complex_F64 c : zeros) {
        if (!c.isReal() && Math.abs(c.imaginary) > 1e-10)
            continue;
        DMatrixRMaj F = solutions.grow();
        double a = c.real;
        double b = 1 - c.real;
        for (int i = 0; i < 9; i++) {
            F.data[i] = a * F1.data[i] + b * F2.data[i];
        }
        // that the first two singular values are zero and the last one is zero
        if (!computeFundamental && !projectOntoEssential(F)) {
            solutions.removeTail();
        }
    }
}

Example 19 with DMatrixRMaj

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

the class ExampleFundamentalMatrix method robustFundamental.

/**
 * Given a set of noisy observations, compute the Fundamental matrix while removing
 * the noise.
 *
 * @param matches List of associated features between the two images
 * @param inliers List of feature pairs that were determined to not be noise.
 * @return The found fundamental matrix.
 */
public static DMatrixRMaj robustFundamental(List<AssociatedPair> matches, List<AssociatedPair> inliers) {
    // used to create and copy new instances of the fit model
    ModelManager<DMatrixRMaj> managerF = new ModelManagerEpipolarMatrix();
    // Select which linear algorithm is to be used.  Try playing with the number of remove ambiguity points
    Estimate1ofEpipolar estimateF = FactoryMultiView.computeFundamental_1(EnumFundamental.LINEAR_7, 2);
    // Wrapper so that this estimator can be used by the robust estimator
    GenerateEpipolarMatrix generateF = new GenerateEpipolarMatrix(estimateF);
    // How the error is measured
    DistanceFromModelResidual<DMatrixRMaj, AssociatedPair> errorMetric = new DistanceFromModelResidual<>(new FundamentalResidualSampson());
    // Use RANSAC to estimate the Fundamental matrix
    ModelMatcher<DMatrixRMaj, AssociatedPair> robustF = new Ransac<>(123123, managerF, generateF, errorMetric, 6000, 0.1);
    // Estimate the fundamental matrix while removing outliers
    if (!robustF.process(matches))
        throw new IllegalArgumentException("Failed");
    // save the set of features that were used to compute the fundamental matrix
    inliers.addAll(robustF.getMatchSet());
    // Improve the estimate of the fundamental matrix using non-linear optimization
    DMatrixRMaj F = new DMatrixRMaj(3, 3);
    ModelFitter<DMatrixRMaj, AssociatedPair> refine = FactoryMultiView.refineFundamental(1e-8, 400, EpipolarError.SAMPSON);
    if (!refine.fitModel(inliers, robustF.getModelParameters(), F))
        throw new IllegalArgumentException("Failed");
    // Return the solution
    return F;
}

Example 20 with DMatrixRMaj

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

the class ExampleFundamentalMatrix method main.

public static void main(String[] args) {
    String dir = UtilIO.pathExample("structure/");
    BufferedImage imageA = UtilImageIO.loadImage(dir, "undist_cyto_01.jpg");
    BufferedImage imageB = UtilImageIO.loadImage(dir, "undist_cyto_02.jpg");
    List<AssociatedPair> matches = computeMatches(imageA, imageB);
    // Where the fundamental matrix is stored
    DMatrixRMaj F;
    // List of matches that matched the model
    List<AssociatedPair> inliers = new ArrayList<>();
    // estimate and print the results using a robust and simple estimator
    // The results should be difference since there are many false associations in the simple model
    // Also note that the fundamental matrix is only defined up to a scale factor.
    F = robustFundamental(matches, inliers);
    System.out.println("Robust");
    F.print();
    F = simpleFundamental(matches);
    System.out.println("Simple");
    F.print();
    // display the inlier matches found using the robust estimator
    AssociationPanel panel = new AssociationPanel(20);
    panel.setAssociation(inliers);
    panel.setImages(imageA, imageB);
    ShowImages.showWindow(panel, "Inlier Pairs");
}