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 . 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 < 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 ) ; }
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 .