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

Example 96 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 intrinsicLeft Intrinsic camera parameters
 * @param rectifiedLeft Output rectified image for left camera.
 * @param rectifiedRight Output rectified image for right camera.
 * @param rectifiedMask Mask that indicates invalid pixels in rectified image. 1 = valid, 0 = invalid
 * @param rectifiedK Output camera calibration matrix for rectified camera
 */
public static <T extends ImageBase<T>> void rectifyImages(T distortedLeft, T distortedRight, Se3_F64 leftToRight, CameraPinholeBrown intrinsicLeft, CameraPinholeBrown intrinsicRight, T rectifiedLeft, T rectifiedRight, GrayU8 rectifiedMask, DMatrixRMaj rectifiedK, DMatrixRMaj rectifiedR) {
    RectifyCalibrated rectifyAlg = RectifyImageOps.createCalibrated();
    // original camera calibration matrices
    DMatrixRMaj K1 = PerspectiveOps.pinholeToMatrix(intrinsicLeft, (DMatrixRMaj) null);
    DMatrixRMaj K2 = PerspectiveOps.pinholeToMatrix(intrinsicRight, (DMatrixRMaj) null);
    rectifyAlg.process(K1, new Se3_F64(), K2, leftToRight);
    // rectification matrix for each image
    DMatrixRMaj rect1 = rectifyAlg.getUndistToRectPixels1();
    DMatrixRMaj rect2 = rectifyAlg.getUndistToRectPixels2();
    rectifiedR.setTo(rectifyAlg.getRectifiedRotation());
    // New calibration matrix,
    rectifiedK.setTo(rectifyAlg.getCalibrationMatrix());
    // Adjust the rectification to make the view area more useful
    ImageDimension rectShape = new ImageDimension();
    RectifyImageOps.fullViewLeft(intrinsicLeft, rect1, rect2, rectifiedK, rectShape);
    // RectifyImageOps.allInsideLeft(intrinsicLeft, rect1, rect2, rectifiedK, rectShape);
    // Taking in account the relative rotation between the image axis and the baseline is important in
    // this scenario since a person can easily hold the camera at an odd angle. If you don't adjust
    // the rectified image size you might end up with a lot of wasted pixels and a low resolution model!
    rectifiedLeft.reshape(rectShape.width, rectShape.height);
    rectifiedRight.reshape(rectShape.width, rectShape.height);
    // 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);
    // Extending the image prevents a harsh edge reducing false matches at the image border
    // SKIP is another option, possibly a tinny bit faster, but has a harsh edge which will need to be filtered
    ImageDistort<T, T> distortLeft = RectifyDistortImageOps.rectifyImage(intrinsicLeft, rect1_F32, BorderType.EXTENDED, distortedLeft.getImageType());
    ImageDistort<T, T> distortRight = RectifyDistortImageOps.rectifyImage(intrinsicRight, rect2_F32, BorderType.EXTENDED, distortedRight.getImageType());
    distortLeft.apply(distortedLeft, rectifiedLeft, rectifiedMask);
    distortRight.apply(distortedRight, rectifiedRight);
}

Example 97 with DMatrixRMaj

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

the class GenericCalibrationZhang99 method optimizedParam_noisy.

void optimizedParam_noisy(double noiseMagnitude) {
    for (CameraConfig config : createCamera(rand)) {
        CalibInputs inputs = createInputs(config.model, 3, rand);
        Zhang99Camera zhangCamera = createGenerator(config, inputs.layout);
        // Add noise to the initial pinhole camera estimate
        DMatrixRMaj K = cameraToK(config);
        K.data[0] += (rand.nextDouble() - 0.5) * noiseMagnitude;
        K.data[4] += (rand.nextDouble() - 0.5) * noiseMagnitude;
        var alg = new CalibrationPlanarGridZhang99(inputs.layout, zhangCamera);
        alg.convertIntoBundleStructure(inputs.worldToViews, K, inputs.homographies, inputs.observations);
        assertTrue(alg.performBundleAdjustment());
        // verify results using errors
        List<ImageResults> errors = alg.computeErrors();
        for (int i = 0; i < errors.size(); i++) {
            assertEquals(0, errors.get(i).meanError, 0.01);
        }
    }
}

Example 98 with DMatrixRMaj

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

the class TestZhang99CalibrationMatrixFromHomography method withNoSkew.

@Test
void withNoSkew() {
    // try different sizes
    for (int N = 2; N <= 5; N++) {
        DMatrixRMaj K = GenericCalibrationGrid.createStandardCalibration();
        // force skew to zero
        K.set(0, 1, 0);
        homographies = GenericCalibrationGrid.createHomographies(K, N, rand);
        Zhang99CalibrationMatrixFromHomographies alg = new Zhang99CalibrationMatrixFromHomographies(true);
        alg.process(homographies);
        DMatrixRMaj K_found = alg.getCalibrationMatrix();
        checkK(K, K_found);
    }
}

Example 99 with DMatrixRMaj

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

the class TestBundleAdjustmentProjectiveSchurJacobian_DDRM method compareToNumerical_Homogenous.

@Test
void compareToNumerical_Homogenous() {
    SceneStructureProjective structure = createSceneH(rand);
    SceneObservations observations = createObservations(rand, structure);
    double[] param = new double[structure.getParameterCount()];
    new CodecSceneStructureProjective().encode(structure, param);
    BundleAdjustmentProjectiveSchurJacobian_DDRM alg = new BundleAdjustmentProjectiveSchurJacobian_DDRM();
    FunctionNtoMxN<DMatrixRMaj> jac = new SchurJacobian_to_NtoMxN.DDRM(alg);
    BundleAdjustmentProjectiveResidualFunction func = new BundleAdjustmentProjectiveResidualFunction();
    alg.configure(structure, observations);
    func.configure(structure, observations);
    // DerivativeChecker.jacobianPrint(func, jac, param, 0.1 );
    assertTrue(DerivativeChecker.jacobian(func, jac, param, 0.1));
}

Example 100 with DMatrixRMaj

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

the class CommonFundamentalChecks method checkEpipolarMatrix.

public void checkEpipolarMatrix(int N, boolean isFundamental) {
    init(100, isFundamental);
    // run several trials with different inputs of size N
    for (int trial = 0; trial < 20; trial++) {
        List<AssociatedPair> pairs = randomPairs(N);
        computeFundamental(pairs, solutions);
        // At least one solution should have been found
        assertTrue(solutions.size() > 0);
        int totalMatchedAll = 0;
        int totalPassedMatrix = 0;
        for (DMatrixRMaj F : solutions.toList()) {
            // normalize F to ensure a consistent scale
            CommonOps_DDRM.scale(1.0 / CommonOps_DDRM.elementMaxAbs(F), F);
            // sanity check, F is not zero
            if (NormOps_DDRM.normF(F) <= 0.1)
                continue;
            // the determinant should be zero
            if (Math.abs(CommonOps_DDRM.det(F)) > zeroTol)
                continue;
            totalPassedMatrix++;
            // see if this hypothesis matched all the points
            boolean matchedAll = true;
            for (AssociatedPair p : this.pairs) {
                double val = GeometryMath_F64.innerProd(p.p2, F, p.p1);
                if (Math.abs(val) > zeroTol) {
                    matchedAll = false;
                    break;
                }
            }
            if (matchedAll) {
                totalMatchedAll++;
            }
        }
        // At least one of them should be consistent with the original set of points
        assertTrue(totalMatchedAll > 0);
        assertTrue(totalPassedMatrix > 0);
    }
}