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

Example 11 with Se2_F64

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

the class TestRefinePolygonToGrayLine method fit_noisy_affine.

/**
 * Fit the quad with a noisy initial guess
 */
@Test
public void fit_noisy_affine() {
    // distorted and undistorted views
    Affine2D_F64[] affines = new Affine2D_F64[2];
    affines[0] = new Affine2D_F64();
    affines[1] = new Affine2D_F64(1.3, 0.05, -0.15, 0.87, 0.1, 0.6);
    ConvertTransform_F64.convert(new Se2_F64(0, 0, 0.2), affines[0]);
    rectangles.add(new Rectangle2D_I32(x0, y0, x1, y1));
    for (Class imageType : imageTypes) {
        for (Affine2D_F64 a : affines) {
            // System.out.println(imageType+"  "+a);
            fit_noisy_affine(true, a, imageType);
            fit_noisy_affine(false, a, imageType);
        }
    }
}

Example 12 with Se2_F64

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

the class TestRefinePolygonToGrayLine method fit_perfect_affine.

/**
 * Perfect initial guess.
 */
@Test
public void fit_perfect_affine() {
    // distorted and undistorted views
    Affine2D_F64[] affines = new Affine2D_F64[2];
    affines[0] = new Affine2D_F64();
    affines[1] = new Affine2D_F64(1.3, 0.05, -0.15, 0.87, 0.1, 0.6);
    ConvertTransform_F64.convert(new Se2_F64(0, 0, 0.2), affines[0]);
    rectangles.add(new Rectangle2D_I32(x0, y0, x1, y1));
    for (Class imageType : imageTypes) {
        for (Affine2D_F64 a : affines) {
            // System.out.println(imageType+"  "+a);
            fit_perfect_affine(true, a, imageType);
            fit_perfect_affine(false, a, imageType);
        }
    }
}

Example 13 with Se2_F64

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

the class VisOdomMonoPlaneInfinity method getWorldToCurr3D.

/**
 * Converts 2D motion estimate into a 3D motion estimate
 *
 * @return from world to current frame.
 */
public Se3_F64 getWorldToCurr3D() {
    // compute transform in 2D space
    Se2_F64 currToWorld = getCurrToWorld2D();
    // 2D to 3D coordinates
    currPlaneToWorld3D.getT().set(-currToWorld.T.y, 0, currToWorld.T.x);
    DMatrixRMaj R = currPlaneToWorld3D.getR();
    // set rotation around Y axis.
    // Transpose the 2D transform since the rotation are pointing in opposite directions
    R.unsafe_set(0, 0, currToWorld.c);
    R.unsafe_set(0, 2, -currToWorld.s);
    R.unsafe_set(1, 1, 1);
    R.unsafe_set(2, 0, currToWorld.s);
    R.unsafe_set(2, 2, currToWorld.c);
    currPlaneToWorld3D.invert(worldToCurrPlane3D);
    worldToCurrPlane3D.concat(planeToCamera, worldToCurrCam3D);
    return worldToCurrCam3D;
}

Example 14 with Se2_F64

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

the class FactoryVisualOdometry method monoPlaneInfinity.

/**
 * Monocular plane based visual odometry algorithm which uses both points on the plane and off plane for motion
 * estimation.
 *
 * @see VisOdomMonoPlaneInfinity
 *
 * @param thresholdAdd  New points are spawned when the number of on plane inliers drops below this value.
 * @param thresholdRetire Tracks are dropped when they are not contained in the inlier set for this many frames
 *                        in a row.  Try 2
 * @param inlierPixelTol Threshold used to determine inliers in pixels.  Try 1.5
 * @param ransacIterations Number of RANSAC iterations.  Try 200
 * @param tracker Image feature tracker
 * @param imageType Type of input image it processes
 * @param <T>
 * @return New instance of
 */
public static <T extends ImageGray<T>> MonocularPlaneVisualOdometry<T> monoPlaneInfinity(int thresholdAdd, int thresholdRetire, double inlierPixelTol, int ransacIterations, PointTracker<T> tracker, ImageType<T> imageType) {
    // squared pixel error
    double ransacTOL = inlierPixelTol * inlierPixelTol;
    ModelManagerSe2_F64 manager = new ModelManagerSe2_F64();
    DistancePlane2DToPixelSq distance = new DistancePlane2DToPixelSq();
    GenerateSe2_PlanePtPixel generator = new GenerateSe2_PlanePtPixel();
    ModelMatcher<Se2_F64, PlanePtPixel> motion = new Ransac<>(2323, manager, generator, distance, ransacIterations, ransacTOL);
    VisOdomMonoPlaneInfinity<T> alg = new VisOdomMonoPlaneInfinity<>(thresholdAdd, thresholdRetire, inlierPixelTol, motion, tracker);
    return new MonoPlaneInfinity_to_MonocularPlaneVisualOdometry<>(alg, distance, generator, imageType);
}