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

Example 21 with DMatrixRMaj

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

the class ExampleRectifyUncalibratedStereo method rectify.

/**
 * Rectifies the image using the provided fundamental matrix.  Both the fundamental matrix
 * and set of inliers need to be accurate.  Small errors will cause large distortions.
 *
 * @param F Fundamental matrix
 * @param inliers Set of associated pairs between the two images.
 * @param origLeft Original input image.  Used for output purposes.
 * @param origRight Original input image.  Used for output purposes.
 */
public static void rectify(DMatrixRMaj F, List<AssociatedPair> inliers, BufferedImage origLeft, BufferedImage origRight) {
    // Unrectified images
    Planar<GrayF32> unrectLeft = ConvertBufferedImage.convertFromPlanar(origLeft, null, true, GrayF32.class);
    Planar<GrayF32> unrectRight = ConvertBufferedImage.convertFromPlanar(origRight, null, true, GrayF32.class);
    // storage for rectified images
    Planar<GrayF32> rectLeft = unrectLeft.createSameShape();
    Planar<GrayF32> rectRight = unrectRight.createSameShape();
    // Compute rectification
    RectifyFundamental rectifyAlg = RectifyImageOps.createUncalibrated();
    rectifyAlg.process(F, inliers, origLeft.getWidth(), origLeft.getHeight());
    // rectification matrix for each image
    DMatrixRMaj rect1 = rectifyAlg.getRect1();
    DMatrixRMaj rect2 = rectifyAlg.getRect2();
    // Adjust the rectification to make the view area more useful
    RectifyImageOps.fullViewLeft(origLeft.getWidth(), origLeft.getHeight(), rect1, rect2);
    // RectifyImageOps.allInsideLeft(origLeft.getWidth(),origLeft.getHeight(), rect1, rect2 );
    // undistorted and rectify images
    // TODO simplify code some how
    FMatrixRMaj rect1_F32 = new FMatrixRMaj(3, 3);
    FMatrixRMaj rect2_F32 = new FMatrixRMaj(3, 3);
    ConvertMatrixData.convert(rect1, rect1_F32);
    ConvertMatrixData.convert(rect2, rect2_F32);
    ImageDistort<GrayF32, GrayF32> imageDistortLeft = RectifyImageOps.rectifyImage(rect1_F32, BorderType.SKIP, GrayF32.class);
    ImageDistort<GrayF32, GrayF32> imageDistortRight = RectifyImageOps.rectifyImage(rect2_F32, BorderType.SKIP, GrayF32.class);
    DistortImageOps.distortPL(unrectLeft, rectLeft, imageDistortLeft);
    DistortImageOps.distortPL(unrectRight, rectRight, imageDistortRight);
    // convert for output
    BufferedImage outLeft = ConvertBufferedImage.convertTo(rectLeft, null, true);
    BufferedImage outRight = ConvertBufferedImage.convertTo(rectRight, null, true);
    // show results and draw a horizontal line where the user clicks to see rectification easier
    // Don't worry if the image appears upside down
    ShowImages.showWindow(new RectifiedPairPanel(true, outLeft, outRight), "Rectified");
}

Example 22 with DMatrixRMaj

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

the class ExampleStereoTwoViewsOneCamera method rectifyImages.

/**
 * Remove lens distortion and rectify stereo images
 *
 * @param distortedLeft  Input distorted image from left camera.
 * @param distortedRight Input distorted image from right camera.
 * @param leftToRight    Camera motion from left to right
 * @param intrinsic      Intrinsic camera parameters
 * @param rectifiedLeft  Output rectified image for left camera.
 * @param rectifiedRight Output rectified image for right camera.
 * @param rectifiedK     Output camera calibration matrix for rectified camera
 */
public static void rectifyImages(GrayU8 distortedLeft, GrayU8 distortedRight, Se3_F64 leftToRight, CameraPinholeRadial intrinsic, GrayU8 rectifiedLeft, GrayU8 rectifiedRight, DMatrixRMaj rectifiedK) {
    RectifyCalibrated rectifyAlg = RectifyImageOps.createCalibrated();
    // original camera calibration matrices
    DMatrixRMaj K = PerspectiveOps.calibrationMatrix(intrinsic, (DMatrixRMaj) null);
    rectifyAlg.process(K, new Se3_F64(), K, leftToRight);
    // rectification matrix for each image
    DMatrixRMaj rect1 = rectifyAlg.getRect1();
    DMatrixRMaj rect2 = rectifyAlg.getRect2();
    // New calibration matrix,
    rectifiedK.set(rectifyAlg.getCalibrationMatrix());
    // Adjust the rectification to make the view area more useful
    RectifyImageOps.allInsideLeft(intrinsic, rect1, rect2, rectifiedK);
    // undistorted and rectify images
    FMatrixRMaj rect1_F32 = new FMatrixRMaj(3, 3);
    FMatrixRMaj rect2_F32 = new FMatrixRMaj(3, 3);
    ConvertMatrixData.convert(rect1, rect1_F32);
    ConvertMatrixData.convert(rect2, rect2_F32);
    ImageDistort<GrayU8, GrayU8> distortLeft = RectifyImageOps.rectifyImage(intrinsic, rect1_F32, BorderType.SKIP, distortedLeft.getImageType());
    ImageDistort<GrayU8, GrayU8> distortRight = RectifyImageOps.rectifyImage(intrinsic, rect2_F32, BorderType.SKIP, distortedRight.getImageType());
    distortLeft.apply(distortedLeft, rectifiedLeft);
    distortRight.apply(distortedRight, rectifiedRight);
}

Example 23 with DMatrixRMaj

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

the class UtilOpenCV method save.

public static void save(CameraPinholeRadial model, String fileName) {
    FileStorage fs = new FileStorage(new File(fileName).getAbsolutePath(), FileStorage.WRITE);
    DMatrixRMaj K = PerspectiveOps.calibrationMatrix(model, (DMatrixRMaj) null);
    write(fs, "image_width", model.width);
    write(fs, "image_height", model.height);
    write(fs, "camera_matrix", toMat(K));
    DMatrixRMaj D = new DMatrixRMaj(2 + 3, 1);
    if (model.radial != null) {
        if (model.radial.length > 0)
            D.set(0, 0, model.radial[0]);
        if (model.radial.length > 1)
            D.set(1, 0, model.radial[1]);
        if (model.radial.length > 2)
            D.set(4, 0, model.radial[2]);
    }
    D.set(2, 0, model.t1);
    D.set(3, 0, model.t2);
    write(fs, "distortion_coefficients", toMat(D));
    try {
        fs.close();
    } catch (Exception ignore) {
    }
}

Example 24 with DMatrixRMaj

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

the class CommonTriangulationChecks method createScene.

public void createScene() {
    worldPoint = new Point3D_F64(0.1, -0.2, 4);
    motionWorldToCamera = new ArrayList<>();
    obsPts = new ArrayList<>();
    essential = new ArrayList<>();
    Point3D_F64 cameraPoint = new Point3D_F64();
    for (int i = 0; i < N; i++) {
        // random motion from world to frame 'i'
        Se3_F64 tranWtoI = new Se3_F64();
        if (i > 0) {
            tranWtoI.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rand.nextGaussian() * 0.01, rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.1, null));
            tranWtoI.getT().set(0.2 + rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.01);
        }
        DMatrixRMaj E = MultiViewOps.createEssential(tranWtoI.getR(), tranWtoI.getT());
        SePointOps_F64.transform(tranWtoI, worldPoint, cameraPoint);
        Point2D_F64 o = new Point2D_F64(cameraPoint.x / cameraPoint.z, cameraPoint.y / cameraPoint.z);
        obsPts.add(o);
        motionWorldToCamera.add(tranWtoI);
        essential.add(E);
    }
}

Example 25 with DMatrixRMaj

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

the class TestDistanceSe3SymmetricSq method testIntrinsicParameters.

/**
 * Manually compute the error using a calibration matrix and see if they match
 */
@Test
public void testIntrinsicParameters() {
    // intrinsic camera calibration matrix
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
    DMatrixRMaj K2 = new DMatrixRMaj(3, 3, true, 105, 0.021, 180, 0, 155, 210, 0, 0, 1);
    DMatrixRMaj K_inv = new DMatrixRMaj(3, 3);
    DMatrixRMaj K2_inv = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.invert(K, K_inv);
    CommonOps_DDRM.invert(K2, K2_inv);
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getT().set(0.1, -0.1, 0.01);
    Point3D_F64 X = new Point3D_F64(0.02, -0.05, 3);
    AssociatedPair obs = new AssociatedPair();
    AssociatedPair obsP = new AssociatedPair();
    obs.p1.x = X.x / X.z;
    obs.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.p2.x = X.x / X.z;
    obs.p2.y = X.y / X.z;
    // translate into pixels
    GeometryMath_F64.mult(K, obs.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obs.p2, obsP.p2);
    // add some noise
    obsP.p1.x += 0.25;
    obsP.p1.y += 0.25;
    obsP.p2.x -= 0.25;
    obsP.p2.y -= 0.25;
    // convert noisy into normalized coordinates
    GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    // triangulate the point's position given noisy data
    LineParametric3D_F64 rayA = new LineParametric3D_F64();
    LineParametric3D_F64 rayB = new LineParametric3D_F64();
    rayA.slope.set(obsP.p1.x, obsP.p1.y, 1);
    rayB.p.set(-0.1, 0.1, -0.01);
    rayB.slope.set(obsP.p2.x, obsP.p2.y, 1);
    ClosestPoint3D_F64.closestPoint(rayA, rayB, X);
    // compute predicted given noisy triangulation
    AssociatedPair ugh = new AssociatedPair();
    ugh.p1.x = X.x / X.z;
    ugh.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    ugh.p2.x = X.x / X.z;
    ugh.p2.y = X.y / X.z;
    // convert everything into pixels
    GeometryMath_F64.mult(K, ugh.p1, ugh.p1);
    GeometryMath_F64.mult(K2, ugh.p2, ugh.p2);
    GeometryMath_F64.mult(K, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obsP.p2, obsP.p2);
    double dx1 = ugh.p1.x - obsP.p1.x;
    double dy1 = ugh.p1.y - obsP.p1.y;
    double dx2 = ugh.p2.x - obsP.p2.x;
    double dy2 = ugh.p2.y - obsP.p2.y;
    double error = dx1 * dx1 + dy1 * dy1 + dx2 * dx2 + dy2 * dy2;
    // convert noisy back into normalized coordinates
    GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    DistanceSe3SymmetricSq alg = new DistanceSe3SymmetricSq(triangulate);
    alg.setIntrinsic(K.get(0, 0), K.get(1, 1), K.get(0, 1), K2.get(0, 0), K2.get(1, 1), K2.get(0, 1));
    alg.setModel(keyToCurr);
    assertEquals(error, alg.computeDistance(obsP), 1e-8);
}