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

Example 51 with DMatrixRMaj

use of org.ejml.data.DMatrixRMaj in project narchy by automenta.

the class ExampleStereoTwoViewsOneCamera method update.

protected boolean update(BufferedImage inLeft) {
    // ShowImages.showGrid(2, "source", prev, next);
    // matched features between the two images
    computeMatches(prev, next);
    // convert from pixel coordinates into normalized image coordinates
    convertToNormalizedCoordinates(matchedFeatures, intrinsic);
    // Robustly estimate camera motion
    Se3_F64 leftToRight = estimateCameraMotion(intrinsic);
    if (leftToRight == null)
        // no motion
        return false;
    drawInliers(intrinsic, inliers);
    // Rectify and remove lens distortion for stereo processing
    DMatrixRMaj rectifiedK = new DMatrixRMaj(3, 3);
    GrayU8 rectifiedLeft = distortedPrev.createSameShape();
    GrayU8 rectifiedRight = distortedNext.createSameShape();
    rectifyImages(distortedPrev, distortedNext, leftToRight, intrinsic, rectifiedLeft, rectifiedRight, rectifiedK);
    // compute disparity
    StereoDisparity<GrayS16, GrayF32> disparityAlg = FactoryStereoDisparity.regionSubpixelWta(DisparityAlgorithms.RECT_FIVE, minDisparity, maxDisparity, 3, 3, 0, 15, 0.1, GrayS16.class);
    // Apply the Laplacian across the image to add extra resistance to changes in lighting or camera gain
    GrayS16 derivLeft = new GrayS16(rectifiedLeft.width, rectifiedLeft.height);
    LaplacianEdge.process(rectifiedLeft, derivLeft);
    GrayS16 derivRight = new GrayS16(rectifiedRight.width, rectifiedRight.height);
    LaplacianEdge.process(rectifiedRight, derivRight);
    // process and return the results
    disparityAlg.process(derivLeft, derivRight);
    GrayF32 disparity = disparityAlg.getDisparity();
    // show results
    BufferedImage visualized = VisualizeImageData.disparity(disparity, null, minDisparity, maxDisparity, 0);
    ShowImages.showWindow(visualized, "Disparity");
    // BufferedImage outLeft = ConvertBufferedImage.convertTo(rectifiedLeft, null);
    // BufferedImage outRight = ConvertBufferedImage.convertTo(rectifiedRight, null);
    // ShowImages.showWindow(new RectifiedPairPanel(true, outLeft, outRight), "Rectification");
    double baseline = leftToRight.getT().norm();
    gui.configure(baseline, rectifiedK, new DoNothing2Transform2_F64(), minDisparity, maxDisparity);
    gui.process(leftToRight, disparity, inLeft);
    return true;
}

Example 52 with DMatrixRMaj

use of org.ejml.data.DMatrixRMaj in project narchy by automenta.

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();
    // RectifyFundamental rectifyAlg = RectifyImageOps.createUncalibrated();
    // 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 53 with DMatrixRMaj

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

the class SelfCalibrationEssentialGuessAndCheck method computeHomography.

private void computeHomography(double F1, double F2, List<AssociatedPair> observations) {
    DMatrixRMaj K1 = CommonOps_DDRM.diag(F1, F1, 1);
    DMatrixRMaj K2 = CommonOps_DDRM.diag(F2, F2, 1);
    calibrator.process(K1, K2, observations);
    rectifyingHomography.setTo(calibrator.getCalibrationHomography());
}

Example 54 with DMatrixRMaj

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

the class ProjectiveStructureFromHomographies method constructLinearSystem.

/**
 * Constructs the linear systems. Unknowns are sorted in index order. structure (3D points) are first followed
 * by unknown t from projective.
 */
void constructLinearSystem(List<DMatrixRMaj> homographies, List<List<PointIndex2D_F64>> observations) {
    // parameters are encoded points first then the
    int startView = totalFeatures * 3;
    A.reshape(numEquations, numUnknown);
    DMatrixRMaj H = new DMatrixRMaj(3, 3);
    Point2D_F64 p = new Point2D_F64();
    int row = 0;
    for (int viewIdx = 0; viewIdx < homographies.size(); viewIdx++) {
        N.apply(homographies.get(viewIdx), H);
        int colView = startView + viewIdx * 3;
        List<PointIndex2D_F64> obs = observations.get(viewIdx);
        for (int i = 0; i < obs.size(); i++) {
            PointIndex2D_F64 p_pixel = obs.get(i);
            N.apply(p_pixel.p, p);
            // column this feature is at
            int col = p_pixel.index * 3;
            // x component of pixel
            // A(row,colView) =  ...
            A.data[row * A.numCols + col] = p.x * H.get(2, 0) - H.get(0, 0);
            A.data[row * A.numCols + col + 1] = p.x * H.get(2, 1) - H.get(0, 1);
            A.data[row * A.numCols + col + 2] = p.x * H.get(2, 2) - H.get(0, 2);
            A.data[row * A.numCols + colView] = -1;
            A.data[row * A.numCols + colView + 1] = 0;
            A.data[row * A.numCols + colView + 2] = p.x;
            // y component of pixel
            row += 1;
            A.data[row * A.numCols + col] = p.y * H.get(2, 0) - H.get(1, 0);
            A.data[row * A.numCols + col + 1] = p.y * H.get(2, 1) - H.get(1, 1);
            A.data[row * A.numCols + col + 2] = p.y * H.get(2, 2) - H.get(1, 2);
            A.data[row * A.numCols + colView] = 0;
            A.data[row * A.numCols + colView + 1] = -1;
            A.data[row * A.numCols + colView + 2] = p.y;
            row += 1;
        }
    }
}

Example 55 with DMatrixRMaj

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

the class ProjectiveStructureFromHomographies method getCameraMatrix.

public void getCameraMatrix(int viewIdx, DMatrixRMaj P) {
    DMatrixRMaj H = homographies.get(viewIdx);
    int row = totalFeatures * 3 + viewIdx * 3;
    tmp.x = B.unsafe_get(row, 0);
    tmp.y = B.unsafe_get(row + 1, 0);
    tmp.z = B.unsafe_get(row + 2, 0);
    N.remove(tmp, tmp);
    CommonOps_DDRM.insert(H, P, 0, 0);
    P.set(0, 3, tmp.x);
    P.set(1, 3, tmp.y);
    P.set(2, 3, tmp.z);
}