Examples with Se3_F64 georegression.struct.se.Se3_F64 used on opensource projects

Example 31 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class MonoOverhead_to_MonocularPlaneVisualOdometry method getCameraToWorld.

@Override
public Se3_F64 getCameraToWorld() {
    Se3_F64 worldToCamera = alg.getWorldToCurr3D();
    worldToCamera.invert(cameraToWorld);
    return cameraToWorld;
}

Example 32 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class CalibrateStereoPlanarGuiApp method setRectification.

/**
 * Computes stereo rectification and then passes the distortion along to the gui.
 */
private void setRectification(final StereoParameters param) {
    // calibration matrix for left and right camera
    DMatrixRMaj K1 = PerspectiveOps.calibrationMatrix(param.getLeft(), (DMatrixRMaj) null);
    DMatrixRMaj K2 = PerspectiveOps.calibrationMatrix(param.getRight(), (DMatrixRMaj) null);
    RectifyCalibrated rectify = RectifyImageOps.createCalibrated();
    rectify.process(K1, new Se3_F64(), K2, param.getRightToLeft().invert(null));
    final DMatrixRMaj rect1 = rectify.getRect1();
    final DMatrixRMaj rect2 = rectify.getRect2();
    SwingUtilities.invokeLater(new Runnable() {

        public void run() {
            gui.setRectification(param.getLeft(), rect1, param.getRight(), rect2);
        }
    });
    gui.repaint();
}

Example 33 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class VisualizeStereoDisparity method rectifyInputImages.

/**
 * Removes distortion and rectifies images.
 */
private void rectifyInputImages() {
    // get intrinsic camera calibration matrices
    DMatrixRMaj K1 = PerspectiveOps.calibrationMatrix(calib.left, (DMatrixRMaj) null);
    DMatrixRMaj K2 = PerspectiveOps.calibrationMatrix(calib.right, (DMatrixRMaj) null);
    // compute rectification matrices
    rectifyAlg.process(K1, new Se3_F64(), K2, calib.getRightToLeft().invert(null));
    DMatrixRMaj rect1 = rectifyAlg.getRect1();
    DMatrixRMaj rect2 = rectifyAlg.getRect2();
    rectK = rectifyAlg.getCalibrationMatrix();
    // adjust view to maximize viewing area while not including black regions
    RectifyImageOps.allInsideLeft(calib.left, rect1, rect2, rectK);
    // compute transforms to apply rectify the images
    leftRectToPixel = transformRectToPixel(calib.left, rect1);
    ImageType<T> imageType = ImageType.single(activeAlg.getInputType());
    // 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<T, T> distortRect1 = RectifyImageOps.rectifyImage(calib.left, rect1_F32, BorderType.SKIP, imageType);
    ImageDistort<T, T> distortRect2 = RectifyImageOps.rectifyImage(calib.right, rect2_F32, BorderType.SKIP, imageType);
    // rectify and undo distortion
    distortRect1.apply(inputLeft, rectLeft);
    distortRect2.apply(inputRight, rectRight);
    rectifiedImages = true;
}

Example 34 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class BenchmarkStabilityFundamental method motionTranslate.

public void motionTranslate() {
    motion = new Se3_F64();
    motion.getT().set(0.2, 0, 0);
}

Example 35 with Se3_F64

use of georegression.struct.se.Se3_F64 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;
    }
}