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

Example 1 with Se2_F64

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

the class FactoryVisualOdometry method monoPlaneOverhead.

/**
 * Monocular plane based visual odometry algorithm which creates a synthetic overhead view and tracks image
 * features inside this synthetic view.
 *
 * @see VisOdomMonoOverheadMotion2D
 *
 * @param cellSize (Overhead) size of ground cells in overhead image in world units
 * @param maxCellsPerPixel (Overhead) Specifies the minimum resolution.  Higher values allow lower resolutions.
 *                         Try 20
 * @param mapHeightFraction (Overhead)  Truncates the overhead view.  Must be from 0 to 1.0.  1.0 includes
 *                          the entire image.
 *
 * @param inlierGroundTol (RANSAC) RANSAC tolerance in overhead image pixels
 * @param ransacIterations (RANSAC) Number of iterations used when estimating motion
 *
 * @param thresholdRetire (2D Motion) Drop tracks if they are not in inliers set for this many turns.
 * @param absoluteMinimumTracks (2D Motion) Spawn tracks if the number of inliers drops below the specified number
 * @param respawnTrackFraction (2D Motion) Spawn tracks if the number of tracks has dropped below this fraction of the
 *                             original number
 * @param respawnCoverageFraction (2D Motion) Spawn tracks if the total coverage drops below this relative fraction
 *
 * @param tracker Image feature tracker
 * @param imageType Type of image being processed
 * @return MonocularPlaneVisualOdometry
 */
public static <T extends ImageGray<T>> MonocularPlaneVisualOdometry<T> monoPlaneOverhead(double cellSize, double maxCellsPerPixel, double mapHeightFraction, double inlierGroundTol, int ransacIterations, int thresholdRetire, int absoluteMinimumTracks, double respawnTrackFraction, double respawnCoverageFraction, PointTracker<T> tracker, ImageType<T> imageType) {
    ImageMotion2D<T, Se2_F64> motion2D = FactoryMotion2D.createMotion2D(ransacIterations, inlierGroundTol * inlierGroundTol, thresholdRetire, absoluteMinimumTracks, respawnTrackFraction, respawnCoverageFraction, false, tracker, new Se2_F64());
    VisOdomMonoOverheadMotion2D<T> alg = new VisOdomMonoOverheadMotion2D<>(cellSize, maxCellsPerPixel, mapHeightFraction, motion2D, imageType);
    return new MonoOverhead_to_MonocularPlaneVisualOdometry<>(alg, imageType);
}

Example 2 with Se2_F64

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

the class TestDetectChessboardFiducial method touchedBorder_rotate.

@Test
public void touchedBorder_rotate() {
    List<Se2_F64> transforms = new ArrayList<>();
    transforms.add(new Se2_F64(0, 0, 0.1));
    transforms.add(new Se2_F64(w - 100, 0, 0.1));
    transforms.add(new Se2_F64(w / 2, -5, 0.4));
    transforms.add(new Se2_F64(w / 2, h - 105, 0.4));
    transforms.add(new Se2_F64(w / 2, h - 100, 0.4));
    transforms.add(new Se2_F64(35, 40, Math.PI / 4));
    transforms.add(new Se2_F64(50, -5, 0.05));
    transforms.add(new Se2_F64(50, -25, 0.05));
    transforms.add(new Se2_F64(50, h - 65, -0.05));
    transforms.add(new Se2_F64(50, h - 90, 0.05));
    offsetX = 0;
    offsetY = 0;
    for (int test = 0; test < transforms.size(); test++) {
        // System.out.println("====================================== Test = "+test);
        transform = transforms.get(test);
        // noise is added to images. Make sure the algorithms aren't brittle
        for (int mc = 0; mc < 5; mc++) {
            basicTest(3, 4, false);
            basicTest(3, 4, true);
        }
    }
}

Example 3 with Se2_F64

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

the class TestLlahOperations method invariantToRotation.

private void invariantToRotation(LlahOperations llahOps) {
    for (int docID = 0; docID < documents.size(); docID++) {
        llahOps.createDocument(documents.get(docID));
    }
    List<Point2D_F64> target = documents.get(2);
    List<LlahOperations.FoundDocument> foundDocuments = new ArrayList<>();
    llahOps.lookupDocuments(target, 8, foundDocuments);
    assertEquals(1, foundDocuments.size());
    DogArray_I32 expected = foundDocuments.get(0).landmarkToDots;
    for (int angleIdx = 0; angleIdx < 12; angleIdx++) {
        var se = new Se2_F64(0, 0, angleIdx * Math.PI / 4);
        // rotate and save into a new list of points
        var rotated = new ArrayList<Point2D_F64>();
        for (var p : target) {
            var d = new Point2D_F64();
            SePointOps_F64.transform(se, p, d);
            rotated.add(d);
        }
        llahOps.lookupDocuments(rotated, 8, foundDocuments);
        assertEquals(1, foundDocuments.size());
        DogArray_I32 found = foundDocuments.get(0).landmarkToDots;
        // Use the number of matches as a finger print
        for (int i = 0; i < expected.size; i++) {
            assertEquals(expected.data[i], found.data[i]);
        }
    }
}

Example 4 with Se2_F64

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

the class VisOdomMonoPlaneInfinity method changeCurrToReference.

/**
 * Updates the relative position of all points so that the current frame is the reference frame. Mathematically
 * this is not needed, but should help keep numbers from getting too large.
 */
private void changeCurrToReference() {
    Se2_F64 keyToCurr = currToKey.invert(null);
    List<PointTrack> all = tracker.getAllTracks(null);
    for (PointTrack t : all) {
        // lint:forbidden ignore_line
        VoTrack p = t.getCookie();
        if (p.onPlane) {
            SePointOps_F64.transform(keyToCurr, p.ground, p.ground);
        } else {
            GeometryMath_F64.rotate(keyToCurr.c, keyToCurr.s, p.ground, p.ground);
        }
    }
    concatMotion();
}

Example 5 with Se2_F64

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

the class FactoryMotion2D method createMotion2D.

/**
 * Estimates the 2D motion of an image using different models.
 *
 * @param ransacIterations Number of RANSAC iterations
 * @param inlierThreshold Threshold which defines an inlier.
 * @param outlierPrune If a feature is an outlier for this many turns in a row it is dropped. Try 2
 * @param absoluteMinimumTracks New features will be respawned if the number of inliers drop below this number.
 * @param respawnTrackFraction If the fraction of current inliers to the original number of inliers drops below
 * this fraction then new features are spawned. Try 0.3
 * @param respawnCoverageFraction If the area covered drops by this fraction then spawn more features. Try 0.8
 * @param refineEstimate Should it refine the model estimate using all inliers.
 * @param tracker Point feature tracker.
 * @param motionModel Instance of the model model used. Affine2D_F64 or Homography2D_F64
 * @param <I> Image input type.
 * @param <IT> Model model
 * @return ImageMotion2D
 */
@SuppressWarnings({ "unchecked", "rawtypes" })
public static <I extends ImageBase<I>, IT extends InvertibleTransform<IT>> ImageMotion2D<I, IT> createMotion2D(int ransacIterations, double inlierThreshold, int outlierPrune, int absoluteMinimumTracks, double respawnTrackFraction, double respawnCoverageFraction, boolean refineEstimate, PointTracker<I> tracker, IT motionModel) {
    ModelManager<IT> manager;
    Factory<ModelGenerator<IT, AssociatedPair>> fitter;
    Factory<DistanceFromModel<IT, AssociatedPair>> distance;
    ModelFitter<IT, AssociatedPair> modelRefiner = null;
    if (motionModel instanceof Homography2D_F64) {
        manager = (ModelManager) new ModelManagerHomography2D_F64();
        if (refineEstimate)
            modelRefiner = (ModelFitter) new GenerateHomographyLinear(true);
        fitter = () -> (ModelGenerator) new GenerateHomographyLinear(true);
        distance = () -> (DistanceFromModel) new DistanceHomographySq();
    } else if (motionModel instanceof Affine2D_F64) {
        manager = (ModelManager) new ModelManagerAffine2D_F64();
        if (refineEstimate)
            modelRefiner = (ModelFitter) new GenerateAffine2D();
        fitter = () -> (ModelGenerator) new GenerateAffine2D();
        distance = () -> (DistanceFromModel) new DistanceAffine2DSq();
    } else if (motionModel instanceof Se2_F64) {
        manager = (ModelManager) new ModelManagerSe2_F64();
        fitter = () -> (ModelGenerator) new GenerateSe2_AssociatedPair(new MotionSe2PointSVD_F64());
        distance = () -> (DistanceFromModel) new DistanceSe2Sq();
    // no refine, already optimal
    } else {
        throw new RuntimeException("Unknown model type: " + motionModel.getClass().getSimpleName());
    }
    ModelMatcherPost<IT, AssociatedPair> modelMatcher = new Ransac<>(123123, ransacIterations, inlierThreshold, manager, AssociatedPair.class);
    modelMatcher.setModel(fitter, distance);
    ImageMotionPointTrackerKey<I, IT> lowlevel = new ImageMotionPointTrackerKey<>(tracker, modelMatcher, modelRefiner, motionModel, outlierPrune);
    ImageMotionPtkSmartRespawn<I, IT> smartRespawn = new ImageMotionPtkSmartRespawn<>(lowlevel, absoluteMinimumTracks, respawnTrackFraction, respawnCoverageFraction);
    return new WrapImageMotionPtkSmartRespawn<>(smartRespawn);
}