Examples with Vector3D_F64 georegression.struct.point.Vector3D_F64 used on opensource projects

Example 41 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class ExampleVisualOdometryStereo method main.

public static void main(String[] args) {
    MediaManager media = DefaultMediaManager.INSTANCE;
    String directory = UtilIO.pathExample("vo/backyard/");
    // load camera description and the video sequence
    StereoParameters stereoParam = CalibrationIO.load(media.openFile(directory + "stereo.yaml"));
    SimpleImageSequence<GrayU8> video1 = media.openVideo(directory + "left.mjpeg", ImageType.single(GrayU8.class));
    SimpleImageSequence<GrayU8> video2 = media.openVideo(directory + "right.mjpeg", ImageType.single(GrayU8.class));
    // specify how the image features are going to be tracked
    PkltConfig configKlt = new PkltConfig();
    configKlt.pyramidScaling = new int[] { 1, 2, 4, 8 };
    configKlt.templateRadius = 3;
    PointTrackerTwoPass<GrayU8> tracker = FactoryPointTrackerTwoPass.klt(configKlt, new ConfigGeneralDetector(600, 3, 1), GrayU8.class, GrayS16.class);
    // computes the depth of each point
    StereoDisparitySparse<GrayU8> disparity = FactoryStereoDisparity.regionSparseWta(0, 150, 3, 3, 30, -1, true, GrayU8.class);
    // declares the algorithm
    StereoVisualOdometry<GrayU8> visualOdometry = FactoryVisualOdometry.stereoDepth(1.5, 120, 2, 200, 50, true, disparity, tracker, GrayU8.class);
    // Pass in intrinsic/extrinsic calibration.  This can be changed in the future.
    visualOdometry.setCalibration(stereoParam);
    // Process the video sequence and output the location plus number of inliers
    while (video1.hasNext()) {
        GrayU8 left = video1.next();
        GrayU8 right = video2.next();
        if (!visualOdometry.process(left, right)) {
            throw new RuntimeException("VO Failed!");
        }
        Se3_F64 leftToWorld = visualOdometry.getCameraToWorld();
        Vector3D_F64 T = leftToWorld.getT();
        System.out.printf("Location %8.2f %8.2f %8.2f      inliers %s\n", T.x, T.y, T.z, inlierPercent(visualOdometry));
    }
}

Example 42 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class TestDistanceHomographyPixelSq method createRandomModel.

@Override
public Homography2D_F64 createRandomModel() {
    double rotX = rand.nextGaussian();
    double rotY = rand.nextGaussian();
    double rotZ = rand.nextGaussian();
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rotX, rotY, rotZ, null);
    Vector3D_F64 T = new Vector3D_F64(0.2, -0.5, 3);
    Vector3D_F64 N = new Vector3D_F64(-0.5, 1, 3);
    // compute the Homography in normalized image coordinates and pixel coordinates
    H = MultiViewOps.createHomography(R, T, 1.0, N);
    H_pixel = MultiViewOps.createHomography(R, T, 1.0, N, K);
    Homography2D_F64 h = new Homography2D_F64();
    UtilHomography_F64.convert(H, h);
    return h;
}

Example 43 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class ExamplePoseOfCalibrationTarget method main.

public static void main(String[] args) {
    // Load camera calibration
    CameraPinholeRadial intrinsic = CalibrationIO.load(UtilIO.pathExample("calibration/mono/Sony_DSC-HX5V_Chess/intrinsic.yaml"));
    LensDistortionNarrowFOV lensDistortion = new LensDistortionRadialTangential(intrinsic);
    // load the video file
    String fileName = UtilIO.pathExample("tracking/chessboard_SonyDSC_01.mjpeg");
    SimpleImageSequence<GrayF32> video = DefaultMediaManager.INSTANCE.openVideo(fileName, ImageType.single(GrayF32.class));
    // DefaultMediaManager.INSTANCE.openCamera(null, 640, 480, ImageType.single(GrayF32.class));
    // Let's use the FiducialDetector interface since it is much easier than coding up
    // the entire thing ourselves.  Look at FiducialDetector's code if you want to understand how it works.
    CalibrationFiducialDetector<GrayF32> detector = FactoryFiducial.calibChessboard(new ConfigChessboard(4, 5, 0.03), GrayF32.class);
    detector.setLensDistortion(lensDistortion, intrinsic.width, intrinsic.height);
    // Get the 2D coordinate of calibration points for visualization purposes
    List<Point2D_F64> calibPts = detector.getCalibrationPoints();
    // Set up visualization
    PointCloudViewer viewer = new PointCloudViewer(intrinsic, 0.01);
    // make the view more interest.  From the side.
    DMatrixRMaj rotY = ConvertRotation3D_F64.rotY(-Math.PI / 2.0, null);
    viewer.setWorldToCamera(new Se3_F64(rotY, new Vector3D_F64(0.75, 0, 1.25)));
    ImagePanel imagePanel = new ImagePanel(intrinsic.width, intrinsic.height);
    viewer.setPreferredSize(new Dimension(intrinsic.width, intrinsic.height));
    PanelGridPanel gui = new PanelGridPanel(1, imagePanel, viewer);
    gui.setMaximumSize(gui.getPreferredSize());
    ShowImages.showWindow(gui, "Calibration Target Pose", true);
    // Allows the user to click on the image and pause
    MousePauseHelper pauseHelper = new MousePauseHelper(gui);
    // saves the target's center location
    List<Point3D_F64> path = new ArrayList<>();
    // Process each frame in the video sequence
    Se3_F64 targetToCamera = new Se3_F64();
    while (video.hasNext()) {
        // detect calibration points
        detector.detect(video.next());
        if (detector.totalFound() == 1) {
            detector.getFiducialToCamera(0, targetToCamera);
            // Visualization.  Show a path with green points and the calibration points in black
            viewer.reset();
            Point3D_F64 center = new Point3D_F64();
            SePointOps_F64.transform(targetToCamera, center, center);
            path.add(center);
            for (Point3D_F64 p : path) {
                viewer.addPoint(p.x, p.y, p.z, 0x00FF00);
            }
            for (int j = 0; j < calibPts.size(); j++) {
                Point2D_F64 p = calibPts.get(j);
                Point3D_F64 p3 = new Point3D_F64(p.x, p.y, 0);
                SePointOps_F64.transform(targetToCamera, p3, p3);
                viewer.addPoint(p3.x, p3.y, p3.z, 0);
            }
        }
        imagePanel.setImage((BufferedImage) video.getGuiImage());
        viewer.repaint();
        imagePanel.repaint();
        BoofMiscOps.pause(30);
        while (pauseHelper.isPaused()) {
            BoofMiscOps.pause(30);
        }
    }
}

Example 44 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class GenericCalibrationGrid method createHomographies.

/**
 * Creates several random uncalibrated homographies
 * @param K Calibration matrix
 * @param N Number of homographies
 */
public static List<DMatrixRMaj> createHomographies(DMatrixRMaj K, int N, Random rand) {
    List<DMatrixRMaj> homographies = new ArrayList<>();
    for (int i = 0; i < N; i++) {
        Vector3D_F64 T = new Vector3D_F64();
        T.x = rand.nextGaussian() * 200;
        T.y = rand.nextGaussian() * 200;
        T.z = rand.nextGaussian() * 50 - 1000;
        double rotX = (rand.nextDouble() - 0.5) * 0.1;
        double rotY = (rand.nextDouble() - 0.5) * 0.1;
        double rotZ = (rand.nextDouble() - 0.5) * 0.1;
        DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rotX, rotY, rotZ, null);
        DMatrixRMaj H = computeHomography(K, R, T);
        homographies.add(H);
    }
    return homographies;
}

Example 45 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class TestZhang99DecomposeHomography method knownCase.

/**
 * Test against a simple known case
 */
@Test
public void knownCase() {
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.02, -0.05, 0.01, null);
    Vector3D_F64 T = new Vector3D_F64(100, 50, -1000);
    DMatrixRMaj K = GenericCalibrationGrid.createStandardCalibration();
    DMatrixRMaj H = GenericCalibrationGrid.computeHomography(K, R, T);
    Zhang99DecomposeHomography alg = new Zhang99DecomposeHomography();
    alg.setCalibrationMatrix(K);
    Se3_F64 motion = alg.decompose(H);
    assertTrue(MatrixFeatures_DDRM.isIdentical(R, motion.getR(), 1e-5));
    assertEquals(T.x, motion.getX(), 1e-5);
}