Search in sources :

Example 1 with StereoSparse3D

use of boofcv.alg.sfm.StereoSparse3D in project BoofCV by lessthanoptimal.

the class FactoryVisualOdometry method stereoDepth.

/**
 * Stereo vision based visual odometry algorithm which runs a sparse feature tracker in the left camera and
 * estimates the range of tracks once when first detected using disparity between left and right cameras.
 *
 * @see VisOdomPixelDepthPnP
 *
 * @param thresholdAdd Add new tracks when less than this number are in the inlier set.  Tracker dependent. Set to
 *                     a value ≤ 0 to add features every frame.
 * @param thresholdRetire Discard a track if it is not in the inlier set after this many updates.  Try 2
 * @param sparseDisparity Estimates the 3D location of features
 * @param imageType Type of image being processed.
 * @return StereoVisualOdometry
 */
public static <T extends ImageGray<T>> StereoVisualOdometry<T> stereoDepth(double inlierPixelTol, int thresholdAdd, int thresholdRetire, int ransacIterations, int refineIterations, boolean doublePass, StereoDisparitySparse<T> sparseDisparity, PointTrackerTwoPass<T> tracker, Class<T> imageType) {
    // Range from sparse disparity
    StereoSparse3D<T> pixelTo3D = new StereoSparse3D<>(sparseDisparity, imageType);
    Estimate1ofPnP estimator = FactoryMultiView.computePnP_1(EnumPNP.P3P_FINSTERWALDER, -1, 2);
    final DistanceModelMonoPixels<Se3_F64, Point2D3D> distance = new PnPDistanceReprojectionSq();
    ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
    EstimatorToGenerator<Se3_F64, Point2D3D> generator = new EstimatorToGenerator<>(estimator);
    // 1/2 a pixel tolerance for RANSAC inliers
    double ransacTOL = inlierPixelTol * inlierPixelTol;
    ModelMatcher<Se3_F64, Point2D3D> motion = new Ransac<>(2323, manager, generator, distance, ransacIterations, ransacTOL);
    RefinePnP refine = null;
    if (refineIterations > 0) {
        refine = FactoryMultiView.refinePnP(1e-12, refineIterations);
    }
    VisOdomPixelDepthPnP<T> alg = new VisOdomPixelDepthPnP<>(thresholdAdd, thresholdRetire, doublePass, motion, pixelTo3D, refine, tracker, null, null);
    return new WrapVisOdomPixelDepthPnP<>(alg, pixelTo3D, distance, imageType);
}
Also used : RefinePnP(boofcv.abst.geo.RefinePnP) Ransac(org.ddogleg.fitting.modelset.ransac.Ransac) EstimatorToGenerator(boofcv.factory.geo.EstimatorToGenerator) StereoSparse3D(boofcv.alg.sfm.StereoSparse3D) Point2D3D(boofcv.struct.geo.Point2D3D) Estimate1ofPnP(boofcv.abst.geo.Estimate1ofPnP) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64) Se3_F64(georegression.struct.se.Se3_F64) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64)

Aggregations

Estimate1ofPnP (boofcv.abst.geo.Estimate1ofPnP)1 RefinePnP (boofcv.abst.geo.RefinePnP)1 StereoSparse3D (boofcv.alg.sfm.StereoSparse3D)1 EstimatorToGenerator (boofcv.factory.geo.EstimatorToGenerator)1 Point2D3D (boofcv.struct.geo.Point2D3D)1 ModelManagerSe3_F64 (georegression.fitting.se.ModelManagerSe3_F64)1 Se3_F64 (georegression.struct.se.Se3_F64)1 Ransac (org.ddogleg.fitting.modelset.ransac.Ransac)1