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 . pnp_1 ( EnumPNP . P3P_FINSTERWALDER , - 1 , 2 ) ; final DistanceFromModelMultiView < 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 . pnpRefine ( 1e-12 , refineIterations ) ; } VisOdomPixelDepthPnP < T > alg = new VisOdomPixelDepthPnP <> ( thresholdAdd , thresholdRetire , doublePass , motion , pixelTo3D , refine , tracker , null , null ) ; return new WrapVisOdomPixelDepthPnP <> ( alg , pixelTo3D , distance , 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 .