Search in sources :

Example 1 with ModelManagerSe3_F64

use of georegression.fitting.se.ModelManagerSe3_F64 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)

Example 2 with ModelManagerSe3_F64

use of georegression.fitting.se.ModelManagerSe3_F64 in project BoofCV by lessthanoptimal.

the class FactoryMultiViewRobust method epipolarRansac.

private static Ransac<Se3_F64, AssociatedPair> epipolarRansac(Estimate1ofEpipolar epipolar, CameraPinholeRadial intrinsic, ConfigRansac ransac) {
    TriangulateTwoViewsCalibrated triangulate = FactoryMultiView.triangulateTwoGeometric();
    ModelManager<Se3_F64> manager = new ModelManagerSe3_F64();
    ModelGenerator<Se3_F64, AssociatedPair> generateEpipolarMotion = new Se3FromEssentialGenerator(epipolar, triangulate);
    DistanceFromModel<Se3_F64, AssociatedPair> distanceSe3 = new DistanceSe3SymmetricSq(triangulate, intrinsic.fx, intrinsic.fy, intrinsic.skew, intrinsic.fx, intrinsic.fy, intrinsic.skew);
    double ransacTOL = ransac.inlierThreshold * ransac.inlierThreshold * 2.0;
    return new Ransac<>(ransac.randSeed, manager, generateEpipolarMotion, distanceSe3, ransac.maxIterations, ransacTOL);
}
Also used : AssociatedPair(boofcv.struct.geo.AssociatedPair) DistanceSe3SymmetricSq(boofcv.alg.geo.robust.DistanceSe3SymmetricSq) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64) Se3FromEssentialGenerator(boofcv.alg.geo.robust.Se3FromEssentialGenerator) Ransac(org.ddogleg.fitting.modelset.ransac.Ransac) TriangulateTwoViewsCalibrated(boofcv.abst.geo.TriangulateTwoViewsCalibrated) Se3_F64(georegression.struct.se.Se3_F64) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64)

Example 3 with ModelManagerSe3_F64

use of georegression.fitting.se.ModelManagerSe3_F64 in project MAVSlam by ecmnet.

the class FactoryMAVOdometry method depthDepthPnP.

/**
 * Depth sensor based visual odometry algorithm which runs a sparse feature tracker in the visual camera and
 * estimates the range of tracks once when first detected using the depth sensor.
 *
 * @see MAVOdomPixelDepthPnP
 *
 * @param thresholdAdd Add new tracks when less than this number are in the inlier set.  Tracker dependent. Set to
 *                     a value &le; 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 sparseDepth Extracts depth of pixels from a depth sensor.
 * @param visualType Type of visual image being processed.
 * @param depthType Type of depth image being processed.
 * @return StereoVisualOdometry
 */
public static <Vis extends ImageGray, Depth extends ImageGray> MAVDepthVisualOdometry<Vis, Depth> depthDepthPnP(double inlierPixelTol, int thresholdAdd, int thresholdRetire, int ransacIterations, int refineIterations, boolean doublePass, DepthSparse3D<Depth> sparseDepth, PointTrackerTwoPass<Vis> tracker, Class<Vis> visualType, Class<Depth> depthType) {
    // Range from sparse disparity
    ImagePixelTo3D pixelTo3D = new DepthSparse3D_to_PixelTo3D<Depth>(sparseDepth);
    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<Se3_F64, Point2D3D>(estimator);
    // 1/2 a pixel tolerance for RANSAC inliers
    double ransacTOL = inlierPixelTol * inlierPixelTol;
    ModelMatcher<Se3_F64, Point2D3D> motion = new Ransac<Se3_F64, Point2D3D>(2323, manager, generator, distance, ransacIterations, ransacTOL);
    RefinePnP refine = null;
    if (refineIterations > 0) {
        refine = FactoryMultiView.refinePnP(1e-12, refineIterations);
    }
    MAVOdomPixelDepthPnP<Vis> alg = new MAVOdomPixelDepthPnP<Vis>(thresholdAdd, thresholdRetire, doublePass, motion, pixelTo3D, refine, tracker, null, null);
    return new MAVOdomPixelDepthPnP_to_DepthVisualOdometry<Vis, Depth>(sparseDepth, alg, distance, ImageType.single(visualType), depthType);
}
Also used : RefinePnP(boofcv.abst.geo.RefinePnP) ImagePixelTo3D(boofcv.abst.sfm.ImagePixelTo3D) DepthSparse3D_to_PixelTo3D(boofcv.abst.sfm.DepthSparse3D_to_PixelTo3D) Ransac(org.ddogleg.fitting.modelset.ransac.Ransac) EstimatorToGenerator(boofcv.factory.geo.EstimatorToGenerator) Point2D3D(boofcv.struct.geo.Point2D3D) PnPDistanceReprojectionSq(boofcv.alg.geo.pose.PnPDistanceReprojectionSq) 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)

Example 4 with ModelManagerSe3_F64

use of georegression.fitting.se.ModelManagerSe3_F64 in project BoofCV by lessthanoptimal.

the class FactoryMultiViewRobust method pnpLMedS.

/**
 * Robust solution to PnP problem using {@link LeastMedianOfSquares LMedS}.  Input observations are
 * in normalized image coordinates.
 *
 * <ul>
 *     <li>Input observations are in normalized image coordinates NOT pixels</li>
 *     <li>Error units are pixels squared.</li>
 * </ul>
 *
 * <p>See code for all the details.</p>
 *
 * @param configPnP PnP parameters.  Can't be null.
 * @param configLMedS Parameters for LMedS.  Can't be null.
 * @return Robust Se3_F64 estimator
 */
public static LeastMedianOfSquares<Se3_F64, Point2D3D> pnpLMedS(ConfigPnP configPnP, ConfigLMedS configLMedS) {
    configPnP.checkValidity();
    configLMedS.checkValidity();
    Estimate1ofPnP estimatorPnP = FactoryMultiView.computePnP_1(configPnP.which, configPnP.epnpIterations, configPnP.numResolve);
    DistanceModelMonoPixels<Se3_F64, Point2D3D> distance = new PnPDistanceReprojectionSq();
    distance.setIntrinsic(configPnP.intrinsic.fx, configPnP.intrinsic.fy, configPnP.intrinsic.skew);
    ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
    EstimatorToGenerator<Se3_F64, Point2D3D> generator = new EstimatorToGenerator<>(estimatorPnP);
    LeastMedianOfSquares<Se3_F64, Point2D3D> lmeds = new LeastMedianOfSquares<>(configLMedS.randSeed, configLMedS.totalCycles, manager, generator, distance);
    lmeds.setErrorFraction(configLMedS.errorFraction);
    return lmeds;
}
Also used : Point2D3D(boofcv.struct.geo.Point2D3D) PnPDistanceReprojectionSq(boofcv.alg.geo.pose.PnPDistanceReprojectionSq) Estimate1ofPnP(boofcv.abst.geo.Estimate1ofPnP) LeastMedianOfSquares(org.ddogleg.fitting.modelset.lmeds.LeastMedianOfSquares) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64) Se3_F64(georegression.struct.se.Se3_F64) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64)

Example 5 with ModelManagerSe3_F64

use of georegression.fitting.se.ModelManagerSe3_F64 in project BoofCV by lessthanoptimal.

the class FactoryMultiViewRobust method pnpRansac.

/**
 * Robust solution to PnP problem using {@link Ransac}.  Input observations are in normalized
 * image coordinates.
 *
 * <p>NOTE: Observations are in normalized image coordinates NOT pixels.</p>
 *
 * <p>See code for all the details.</p>
 *
 * @param pnp PnP parameters.  Can't be null.
 * @param ransac Parameters for RANSAC.  Can't be null.
 * @return Robust Se3_F64 estimator
 */
public static Ransac<Se3_F64, Point2D3D> pnpRansac(ConfigPnP pnp, ConfigRansac ransac) {
    pnp.checkValidity();
    ransac.checkValidity();
    Estimate1ofPnP estimatorPnP = FactoryMultiView.computePnP_1(pnp.which, pnp.epnpIterations, pnp.numResolve);
    DistanceModelMonoPixels<Se3_F64, Point2D3D> distance = new PnPDistanceReprojectionSq();
    distance.setIntrinsic(pnp.intrinsic.fx, pnp.intrinsic.fy, pnp.intrinsic.skew);
    ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
    EstimatorToGenerator<Se3_F64, Point2D3D> generator = new EstimatorToGenerator<>(estimatorPnP);
    // convert from pixels to pixels squared
    double threshold = ransac.inlierThreshold * ransac.inlierThreshold;
    return new Ransac<>(ransac.randSeed, manager, generator, distance, ransac.maxIterations, threshold);
}
Also used : Point2D3D(boofcv.struct.geo.Point2D3D) PnPDistanceReprojectionSq(boofcv.alg.geo.pose.PnPDistanceReprojectionSq) Estimate1ofPnP(boofcv.abst.geo.Estimate1ofPnP) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64) Ransac(org.ddogleg.fitting.modelset.ransac.Ransac) Se3_F64(georegression.struct.se.Se3_F64) ModelManagerSe3_F64(georegression.fitting.se.ModelManagerSe3_F64)

Aggregations

ModelManagerSe3_F64 (georegression.fitting.se.ModelManagerSe3_F64)9 Se3_F64 (georegression.struct.se.Se3_F64)9 Point2D3D (boofcv.struct.geo.Point2D3D)7 Ransac (org.ddogleg.fitting.modelset.ransac.Ransac)7 Estimate1ofPnP (boofcv.abst.geo.Estimate1ofPnP)5 EstimatorToGenerator (boofcv.factory.geo.EstimatorToGenerator)5 TriangulateTwoViewsCalibrated (boofcv.abst.geo.TriangulateTwoViewsCalibrated)4 RefinePnP (boofcv.abst.geo.RefinePnP)3 PnPDistanceReprojectionSq (boofcv.alg.geo.pose.PnPDistanceReprojectionSq)3 EstimateNofPnP (boofcv.abst.geo.EstimateNofPnP)2 DepthSparse3D_to_PixelTo3D (boofcv.abst.sfm.DepthSparse3D_to_PixelTo3D)2 ImagePixelTo3D (boofcv.abst.sfm.ImagePixelTo3D)2 AssociateStereo2D (boofcv.alg.feature.associate.AssociateStereo2D)2 DistanceSe3SymmetricSq (boofcv.alg.geo.robust.DistanceSe3SymmetricSq)2 Se3FromEssentialGenerator (boofcv.alg.geo.robust.Se3FromEssentialGenerator)2 TupleDesc (boofcv.struct.feature.TupleDesc)2 AssociatedPair (boofcv.struct.geo.AssociatedPair)2 Stereo2D3D (boofcv.struct.sfm.Stereo2D3D)2 LeastMedianOfSquares (org.ddogleg.fitting.modelset.lmeds.LeastMedianOfSquares)2 AssociateDescTo2D (boofcv.abst.feature.associate.AssociateDescTo2D)1