use of boofcv.abst.geo.RefinePnP 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);
}
use of boofcv.abst.geo.RefinePnP in project BoofCV by lessthanoptimal.
the class ExamplePnP method estimateNoOutliers.
/**
* Assumes all observations actually match the correct/real 3D point
*/
public Se3_F64 estimateNoOutliers(List<Point2D3D> observations) {
// Compute a single solution using EPNP
// 10 iterations is what JavaDoc recommends, but might need to be tuned.
// 0 test points. This parameters is actually ignored because EPNP only returns a single solution
Estimate1ofPnP pnp = FactoryMultiView.computePnP_1(EnumPNP.EPNP, 10, 0);
Se3_F64 worldToCamera = new Se3_F64();
pnp.process(observations, worldToCamera);
// For some applications the EPNP solution might be good enough, but let's refine it
RefinePnP refine = FactoryMultiView.refinePnP(1e-8, 200);
Se3_F64 refinedWorldToCamera = new Se3_F64();
if (!refine.fitModel(observations, worldToCamera, refinedWorldToCamera))
throw new RuntimeException("Refined failed! Input probably bad...");
return refinedWorldToCamera;
}
use of boofcv.abst.geo.RefinePnP 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 ≤ 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);
}
use of boofcv.abst.geo.RefinePnP in project BoofCV by lessthanoptimal.
the class ExamplePnP method estimateOutliers.
/**
* Uses robust techniques to remove outliers
*/
public Se3_F64 estimateOutliers(List<Point2D3D> observations) {
// We can no longer trust that each point is a real observation. Let's use RANSAC to separate the points
// You will need to tune the number of iterations and inlier threshold!!!
Ransac<Se3_F64, Point2D3D> ransac = FactoryMultiViewRobust.pnpRansac(new ConfigPnP(intrinsic), new ConfigRansac(300, 1.0));
// Observations must be in normalized image coordinates! See javadoc of pnpRansac
if (!ransac.process(observations))
throw new RuntimeException("Probably got bad input data with NaN inside of it");
System.out.println("Inlier size " + ransac.getMatchSet().size());
Se3_F64 worldToCamera = ransac.getModelParameters();
// You will most likely want to refine this solution too. Can make a difference with real world data
RefinePnP refine = FactoryMultiView.refinePnP(1e-8, 200);
Se3_F64 refinedWorldToCamera = new Se3_F64();
// notice that only the match set was passed in
if (!refine.fitModel(ransac.getMatchSet(), worldToCamera, refinedWorldToCamera))
throw new RuntimeException("Refined failed! Input probably bad...");
return refinedWorldToCamera;
}
use of boofcv.abst.geo.RefinePnP in project BoofCV by lessthanoptimal.
the class FactoryVisualOdometry 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 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 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<Vis>, Depth extends ImageGray<Depth>> DepthVisualOdometry<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<>(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<>(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<Vis> alg = new VisOdomPixelDepthPnP<>(thresholdAdd, thresholdRetire, doublePass, motion, pixelTo3D, refine, tracker, null, null);
return new VisOdomPixelDepthPnP_to_DepthVisualOdometry<>(sparseDepth, alg, distance, ImageType.single(visualType), depthType);
}
Aggregations