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

Example 36 with Se3_F64

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

the class CalibratedPoseAndPoint method configure.

/**
 * Specifies the number of views and 3D points being estimated
 *
 * @param numViews Number of camera views observing the points.
 * @param numPoints Number of points observed
 */
public void configure(int numViews, int numPoints) {
    if (worldToCamera.length < numViews) {
        Se3_F64[] temp = new Se3_F64[numViews];
        System.arraycopy(worldToCamera, 0, temp, 0, worldToCamera.length);
        for (int i = worldToCamera.length; i < temp.length; i++) {
            temp[i] = new Se3_F64();
        }
        worldToCamera = temp;
        viewKnown = new boolean[numViews];
    }
    if (points.length < numPoints) {
        Point3D_F64[] temp = new Point3D_F64[numPoints];
        System.arraycopy(points, 0, temp, 0, points.length);
        for (int i = points.length; i < temp.length; i++) {
            temp[i] = new Point3D_F64();
        }
        points = temp;
    }
    this.numPoints = numPoints;
    this.numViews = numViews;
    for (int i = 0; i < numViews; i++) {
        viewKnown[i] = false;
    }
}

Example 37 with Se3_F64

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

the class DecomposeHomography method createMirrorSolution.

private void createMirrorSolution(int origIndex, int index) {
    Se3_F64 origSE = solutionsSE.get(origIndex);
    Vector3D_F64 origN = solutionsN.get(origIndex);
    Se3_F64 se = solutionsSE.get(index);
    Vector3D_F64 N = solutionsN.get(index);
    se.getR().set(origSE.getR());
    N.x = -origN.x;
    N.y = -origN.y;
    N.z = -origN.z;
    Vector3D_F64 origT = origSE.getT();
    Vector3D_F64 T = se.getT();
    T.x = -origT.x;
    T.y = -origT.y;
    T.z = -origT.z;
}

Example 38 with Se3_F64

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

the class ExampleRectifyCalibratedStereo method main.

public static void main(String[] args) {
    String dir = UtilIO.pathExample("calibration/stereo/Bumblebee2_Chess/");
    StereoParameters param = CalibrationIO.load(new File(dir, "stereo.yaml"));
    // load images
    BufferedImage origLeft = UtilImageIO.loadImage(dir, "left05.jpg");
    BufferedImage origRight = UtilImageIO.loadImage(dir, "right05.jpg");
    // distorted images
    Planar<GrayF32> distLeft = ConvertBufferedImage.convertFromPlanar(origLeft, null, true, GrayF32.class);
    Planar<GrayF32> distRight = ConvertBufferedImage.convertFromPlanar(origRight, null, true, GrayF32.class);
    // storage for undistorted + rectified images
    Planar<GrayF32> rectLeft = distLeft.createSameShape();
    Planar<GrayF32> rectRight = distRight.createSameShape();
    // Compute rectification
    RectifyCalibrated rectifyAlg = RectifyImageOps.createCalibrated();
    Se3_F64 leftToRight = param.getRightToLeft().invert(null);
    // original camera calibration matrices
    DMatrixRMaj K1 = PerspectiveOps.calibrationMatrix(param.getLeft(), (DMatrixRMaj) null);
    DMatrixRMaj K2 = PerspectiveOps.calibrationMatrix(param.getRight(), (DMatrixRMaj) null);
    rectifyAlg.process(K1, new Se3_F64(), K2, leftToRight);
    // rectification matrix for each image
    DMatrixRMaj rect1 = rectifyAlg.getRect1();
    DMatrixRMaj rect2 = rectifyAlg.getRect2();
    // New calibration matrix,
    // Both cameras have the same one after rectification.
    DMatrixRMaj rectK = rectifyAlg.getCalibrationMatrix();
    // Adjust the rectification to make the view area more useful
    RectifyImageOps.fullViewLeft(param.left, rect1, rect2, rectK);
    // RectifyImageOps.allInsideLeft(param.left, leftHanded, rect1, rect2, rectK);
    // 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 rectifyImageLeft = RectifyImageOps.rectifyImage(param.getLeft(), rect1_F32, BorderType.SKIP, distLeft.getImageType());
    ImageDistort rectifyImageRight = RectifyImageOps.rectifyImage(param.getRight(), rect2_F32, BorderType.SKIP, distRight.getImageType());
    rectifyImageLeft.apply(distLeft, rectLeft);
    rectifyImageRight.apply(distRight, rectRight);
    // 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
    ListDisplayPanel panel = new ListDisplayPanel();
    panel.addItem(new RectifiedPairPanel(true, origLeft, origRight), "Original");
    panel.addItem(new RectifiedPairPanel(true, outLeft, outRight), "Rectified");
    ShowImages.showWindow(panel, "Stereo Rectification Calibrated", true);
}

Example 39 with Se3_F64

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

the class ExampleStereoDisparity method rectify.

/**
 * Rectified the input images using known calibration.
 */
public static RectifyCalibrated rectify(GrayU8 origLeft, GrayU8 origRight, StereoParameters param, GrayU8 rectLeft, GrayU8 rectRight) {
    // Compute rectification
    RectifyCalibrated rectifyAlg = RectifyImageOps.createCalibrated();
    Se3_F64 leftToRight = param.getRightToLeft().invert(null);
    // original camera calibration matrices
    DMatrixRMaj K1 = PerspectiveOps.calibrationMatrix(param.getLeft(), (DMatrixRMaj) null);
    DMatrixRMaj K2 = PerspectiveOps.calibrationMatrix(param.getRight(), (DMatrixRMaj) null);
    rectifyAlg.process(K1, new Se3_F64(), K2, leftToRight);
    // rectification matrix for each image
    DMatrixRMaj rect1 = rectifyAlg.getRect1();
    DMatrixRMaj rect2 = rectifyAlg.getRect2();
    // New calibration matrix,
    DMatrixRMaj rectK = rectifyAlg.getCalibrationMatrix();
    // Adjust the rectification to make the view area more useful
    RectifyImageOps.allInsideLeft(param.left, rect1, rect2, rectK);
    // 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> imageDistortLeft = RectifyImageOps.rectifyImage(param.getLeft(), rect1_F32, BorderType.SKIP, origLeft.getImageType());
    ImageDistort<GrayU8, GrayU8> imageDistortRight = RectifyImageOps.rectifyImage(param.getRight(), rect2_F32, BorderType.SKIP, origRight.getImageType());
    imageDistortLeft.apply(origLeft, rectLeft);
    imageDistortRight.apply(origRight, rectRight);
    return rectifyAlg;
}

Example 40 with Se3_F64

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