boofcv.factory.sfm.FactoryVisualOdometry Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-sfm Show documentation
Show all versions of boofcv-sfm Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* Copyright (c) 2011-2019, Peter Abeles. All Rights Reserved.
*
* This file is part of BoofCV (http://boofcv.org).
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package boofcv.factory.sfm;
import boofcv.abst.feature.associate.AssociateDescTo2D;
import boofcv.abst.feature.associate.AssociateDescription2D;
import boofcv.abst.feature.associate.EnforceUniqueByScore;
import boofcv.abst.feature.associate.ScoreAssociation;
import boofcv.abst.feature.describe.DescribeRegionPoint;
import boofcv.abst.feature.detdesc.DetectDescribeMulti;
import boofcv.abst.feature.disparity.StereoDisparitySparse;
import boofcv.abst.feature.tracker.PointTracker;
import boofcv.abst.feature.tracker.PointTrackerTwoPass;
import boofcv.abst.geo.Estimate1ofPnP;
import boofcv.abst.geo.EstimateNofPnP;
import boofcv.abst.geo.RefinePnP;
import boofcv.abst.geo.Triangulate2ViewsMetric;
import boofcv.abst.sfm.DepthSparse3D_to_PixelTo3D;
import boofcv.abst.sfm.ImagePixelTo3D;
import boofcv.abst.sfm.d2.ImageMotion2D;
import boofcv.abst.sfm.d3.*;
import boofcv.alg.feature.associate.AssociateMaxDistanceNaive;
import boofcv.alg.feature.associate.AssociateStereo2D;
import boofcv.alg.geo.DistanceFromModelMultiView;
import boofcv.alg.geo.pose.*;
import boofcv.alg.sfm.DepthSparse3D;
import boofcv.alg.sfm.StereoSparse3D;
import boofcv.alg.sfm.d3.*;
import boofcv.alg.sfm.d3.direct.PyramidDirectColorDepth;
import boofcv.alg.sfm.robust.DistancePlane2DToPixelSq;
import boofcv.alg.sfm.robust.GenerateSe2_PlanePtPixel;
import boofcv.factory.feature.associate.FactoryAssociation;
import boofcv.factory.geo.ConfigTriangulation;
import boofcv.factory.geo.EnumPNP;
import boofcv.factory.geo.EstimatorToGenerator;
import boofcv.factory.geo.FactoryMultiView;
import boofcv.factory.transform.pyramid.FactoryPyramid;
import boofcv.struct.feature.TupleDesc;
import boofcv.struct.geo.Point2D3D;
import boofcv.struct.image.ImageBase;
import boofcv.struct.image.ImageGray;
import boofcv.struct.image.ImageType;
import boofcv.struct.image.Planar;
import boofcv.struct.pyramid.ImagePyramid;
import boofcv.struct.sfm.PlanePtPixel;
import boofcv.struct.sfm.Stereo2D3D;
import georegression.fitting.se.ModelManagerSe2_F64;
import georegression.fitting.se.ModelManagerSe3_F64;
import georegression.struct.se.Se2_F64;
import georegression.struct.se.Se3_F64;
import org.ddogleg.fitting.modelset.ModelMatcher;
import org.ddogleg.fitting.modelset.ransac.Ransac;
/**
* Factory for creating visual odometry algorithms.
*
* @author Peter Abeles
*/
public class FactoryVisualOdometry {
/**
* 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
* @return New instance of
*/
public static >
MonocularPlaneVisualOdometry monoPlaneInfinity(int thresholdAdd,
int thresholdRetire,
double inlierPixelTol,
int ransacIterations,
PointTracker tracker,
ImageType 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 motion =
new Ransac<>(2323, manager, generator, distance, ransacIterations, ransacTOL);
VisOdomMonoPlaneInfinity alg =
new VisOdomMonoPlaneInfinity<>(thresholdAdd, thresholdRetire, inlierPixelTol, motion, tracker);
return new MonoPlaneInfinity_to_MonocularPlaneVisualOdometry<>(alg, distance, generator, imageType);
}
/**
* 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 >
MonocularPlaneVisualOdometry monoPlaneOverhead(double cellSize,
double maxCellsPerPixel,
double mapHeightFraction ,
double inlierGroundTol,
int ransacIterations ,
int thresholdRetire ,
int absoluteMinimumTracks,
double respawnTrackFraction,
double respawnCoverageFraction,
PointTracker tracker ,
ImageType imageType ) {
ImageMotion2D motion2D = FactoryMotion2D.createMotion2D(
ransacIterations,inlierGroundTol*inlierGroundTol,thresholdRetire,
absoluteMinimumTracks,respawnTrackFraction,respawnCoverageFraction,false,tracker,new Se2_F64());
VisOdomMonoOverheadMotion2D alg =
new VisOdomMonoOverheadMotion2D<>(cellSize, maxCellsPerPixel, mapHeightFraction, motion2D, imageType);
return new MonoOverhead_to_MonocularPlaneVisualOdometry<>(alg, imageType);
}
/**
* 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 >
StereoVisualOdometry stereoDepth(double inlierPixelTol,
int thresholdAdd,
int thresholdRetire ,
int ransacIterations ,
int refineIterations ,
boolean doublePass ,
StereoDisparitySparse sparseDisparity,
PointTrackerTwoPass tracker ,
Class imageType) {
// Range from sparse disparity
StereoSparse3D pixelTo3D = new StereoSparse3D<>(sparseDisparity, imageType);
Estimate1ofPnP estimator = FactoryMultiView.pnp_1(EnumPNP.P3P_FINSTERWALDER,-1,2);
final DistanceFromModelMultiView distance = new PnPDistanceReprojectionSq();
ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
EstimatorToGenerator generator =
new EstimatorToGenerator<>(estimator);
// 1/2 a pixel tolerance for RANSAC inliers
double ransacTOL = inlierPixelTol * inlierPixelTol;
ModelMatcher motion =
new Ransac<>(2323, manager, generator, distance, ransacIterations, ransacTOL);
RefinePnP refine = null;
if( refineIterations > 0 ) {
refine = FactoryMultiView.pnpRefine(1e-12,refineIterations);
}
VisOdomPixelDepthPnP alg =
new VisOdomPixelDepthPnP<>(thresholdAdd, thresholdRetire, doublePass, motion, pixelTo3D, refine, tracker, null, null);
return new WrapVisOdomPixelDepthPnP<>(alg, pixelTo3D, distance, imageType);
}
/**
* 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 , Depth extends ImageGray>
DepthVisualOdometry depthDepthPnP(double inlierPixelTol,
int thresholdAdd,
int thresholdRetire ,
int ransacIterations ,
int refineIterations ,
boolean doublePass ,
DepthSparse3D sparseDepth,
PointTrackerTwoPass tracker ,
Class visualType , Class depthType ) {
// Range from sparse disparity
ImagePixelTo3D pixelTo3D = new DepthSparse3D_to_PixelTo3D<>(sparseDepth);
Estimate1ofPnP estimator = FactoryMultiView.pnp_1(EnumPNP.P3P_FINSTERWALDER,-1,2);
final DistanceFromModelMultiView distance = new PnPDistanceReprojectionSq();
ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
EstimatorToGenerator generator = new EstimatorToGenerator<>(estimator);
// 1/2 a pixel tolerance for RANSAC inliers
double ransacTOL = inlierPixelTol * inlierPixelTol;
ModelMatcher motion =
new Ransac<>(2323, manager, generator, distance, ransacIterations, ransacTOL);
RefinePnP refine = null;
if( refineIterations > 0 ) {
refine = FactoryMultiView.pnpRefine(1e-12,refineIterations);
}
VisOdomPixelDepthPnP alg = new VisOdomPixelDepthPnP<>
(thresholdAdd, thresholdRetire, doublePass, motion, pixelTo3D, refine, tracker, null, null);
return new VisOdomPixelDepthPnP_to_DepthVisualOdometry<>
(sparseDepth, alg, distance, ImageType.single(visualType), depthType);
}
/**
* Creates a stereo visual odometry algorithm that independently tracks features in left and right camera.
*
* @see VisOdomDualTrackPnP
*
* @param thresholdAdd When the number of inliers is below this number new features are detected
* @param thresholdRetire When a feature has not been in the inlier list for this many ticks it is dropped
* @param inlierPixelTol Tolerance in pixels for defining an inlier during robust model matching. Typically 1.5
* @param epipolarPixelTol Tolerance in pixels for enforcing the epipolar constraint
* @param ransacIterations Number of iterations performed by RANSAC. Try 300 or more.
* @param refineIterations Number of iterations done during non-linear optimization. Try 50 or more.
* @param trackerLeft Tracker used for left camera
* @param trackerRight Tracker used for right camera
* @param descriptor Describes points
* @param describeRadius Radius passed in when describing points. Try 11.0
* @param imageType Type of image being processed
* @return Stereo visual odometry algorithm.
*/
public static , Desc extends TupleDesc>
StereoVisualOdometry stereoDualTrackerPnP(int thresholdAdd, int thresholdRetire,
double inlierPixelTol,
double epipolarPixelTol,
int ransacIterations,
int refineIterations,
PointTracker trackerLeft, PointTracker trackerRight,
DescribeRegionPoint descriptor,
double describeRadius,
Class imageType)
{
EstimateNofPnP pnp = FactoryMultiView.pnp_N(EnumPNP.P3P_FINSTERWALDER, -1);
DistanceFromModelMultiView distanceMono = new PnPDistanceReprojectionSq();
PnPStereoDistanceReprojectionSq distanceStereo = new PnPStereoDistanceReprojectionSq();
PnPStereoEstimator pnpStereo = new PnPStereoEstimator(pnp,distanceMono,0);
ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
EstimatorToGenerator generator = new EstimatorToGenerator<>(pnpStereo);
// Pixel tolerance for RANSAC inliers - euclidean error squared from left + right images
double ransacTOL = 2*inlierPixelTol*inlierPixelTol;
ModelMatcher motion =
new Ransac<>(2323, manager, generator, distanceStereo, ransacIterations, ransacTOL);
RefinePnPStereo refinePnP = null;
Class descType = descriptor.getDescriptionType();
ScoreAssociation scorer = FactoryAssociation.defaultScore(descType);
AssociateStereo2D associateStereo = new AssociateStereo2D<>(scorer, epipolarPixelTol, descType);
// need to make sure associations are unique
AssociateDescription2D associateUnique = associateStereo;
if( !associateStereo.uniqueDestination() || !associateStereo.uniqueSource() ) {
associateUnique = new EnforceUniqueByScore.Describe2D<>(associateStereo, true, true);
}
if( refineIterations > 0 ) {
refinePnP = new PnPStereoRefineRodrigues(1e-12,refineIterations);
}
Triangulate2ViewsMetric triangulate = FactoryMultiView.triangulate2ViewMetric(
new ConfigTriangulation(ConfigTriangulation.Type.GEOMETRIC));
VisOdomDualTrackPnP alg = new VisOdomDualTrackPnP<>(thresholdAdd, thresholdRetire, epipolarPixelTol,
trackerLeft, trackerRight, descriptor, associateUnique, triangulate, motion, refinePnP);
alg.setDescribeRadius(describeRadius);
return new WrapVisOdomDualTrackPnP<>(pnpStereo, distanceMono, distanceStereo, associateStereo, alg, refinePnP, imageType);
}
/**
* Stereo visual odometry which uses the two most recent stereo observations (total of four views) to estimate
* motion.
*
* @see VisOdomQuadPnP
*
* @param inlierPixelTol Pixel tolerance for RANSAC inliers - Euclidean distance
* @param epipolarPixelTol Feature association tolerance in pixels.
* @param maxDistanceF2F Maximum allowed distance between two features in pixels
* @param maxAssociationError Maxium error between two features when associating.
* @param ransacIterations Number of iterations RANSAC will perform
* @param refineIterations Number of refinement iterations
* @param detector Which feature detector to use
* @param imageType Type of input image
*/
public static ,Desc extends TupleDesc>
StereoVisualOdometry stereoQuadPnP( double inlierPixelTol ,
double epipolarPixelTol ,
double maxDistanceF2F,
double maxAssociationError,
int ransacIterations ,
int refineIterations ,
DetectDescribeMulti detector,
Class imageType )
{
EstimateNofPnP pnp = FactoryMultiView.pnp_N(EnumPNP.P3P_FINSTERWALDER, -1);
DistanceFromModelMultiView distanceMono = new PnPDistanceReprojectionSq();
PnPStereoDistanceReprojectionSq distanceStereo = new PnPStereoDistanceReprojectionSq();
PnPStereoEstimator pnpStereo = new PnPStereoEstimator(pnp,distanceMono,0);
ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
EstimatorToGenerator generator = new EstimatorToGenerator<>(pnpStereo);
// euclidean error squared from left + right images
double ransacTOL = 2*inlierPixelTol * inlierPixelTol;
ModelMatcher motion =
new Ransac<>(2323, manager, generator, distanceStereo, ransacIterations, ransacTOL);
RefinePnPStereo refinePnP = null;
if( refineIterations > 0 ) {
refinePnP = new PnPStereoRefineRodrigues(1e-12,refineIterations);
}
Class descType = detector.getDescriptionType();
ScoreAssociation scorer = FactoryAssociation.defaultScore(descType);
// TODO need a better way to keep track of what error is squared and not
AssociateDescription2D assocSame;
if( maxDistanceF2F > 0 ) {
AssociateMaxDistanceNaive a = new AssociateMaxDistanceNaive<>(scorer, true, maxAssociationError);
a.setSquaredDistance(true);
a.setMaxDistance(maxDistanceF2F);
assocSame = a;
} else {
assocSame = new AssociateDescTo2D<>(FactoryAssociation.greedy(scorer, maxAssociationError, true));
}
AssociateStereo2D associateStereo = new AssociateStereo2D<>(scorer, epipolarPixelTol, descType);
Triangulate2ViewsMetric triangulate = FactoryMultiView.triangulate2ViewMetric(
new ConfigTriangulation(ConfigTriangulation.Type.GEOMETRIC));
associateStereo.setMaxScoreThreshold(maxAssociationError);
VisOdomQuadPnP alg = new VisOdomQuadPnP<>(
detector, assocSame, associateStereo, triangulate, motion, refinePnP);
return new WrapVisOdomQuadPnP<>(alg, refinePnP, associateStereo, distanceStereo, distanceMono, imageType);
}
/**
* Wraps around a {@link StereoVisualOdometry} instance and will rescale the input images and adjust the cameras
* intrinsic parameters automatically. Rescaling input images is often an easy way to improve runtime performance
* with a minimal hit on pose accuracy.
*
* @param vo Visual odometry algorithm which is being wrapped
* @param scaleFactor Scale factor that the image should be reduced by, Try 0.5 for half size.
* @param Image type
* @return StereoVisualOdometry
*/
public static > StereoVisualOdometry scaleInput( StereoVisualOdometry vo , double scaleFactor )
{
return new StereoVisualOdometryScaleInput<>(vo, scaleFactor);
}
/**
* Wraps around a {@link MonocularPlaneVisualOdometry} instance and will rescale the input images and adjust the cameras
* intrinsic parameters automatically. Rescaling input images is often an easy way to improve runtime performance
* with a minimal hit on pose accuracy.
*
* @param vo Visual odometry algorithm which is being wrapped
* @param scaleFactor Scale factor that the image should be reduced by, Try 0.5 for half size.
* @param Image type
* @return StereoVisualOdometry
*/
public static > MonocularPlaneVisualOdometry scaleInput( MonocularPlaneVisualOdometry vo , double scaleFactor )
{
return new MonocularPlaneVisualOdometryScaleInput<>(vo, scaleFactor);
}
public static , Depth extends ImageGray>
DepthVisualOdometry,Depth> depthDirect( DepthSparse3D sparse3D,
ImageType> visualType , Class depthType)
{
ImagePyramid> pyramid = FactoryPyramid.discreteGaussian(new int[]{1,2,4},
-1,2,false, visualType);
PyramidDirectColorDepth alg = new PyramidDirectColorDepth<>(pyramid);
return new PyramidDirectColorDepth_to_DepthVisualOdometry<>(sparse3D,alg,depthType);
}
}