* "Real-Time Visual Odometry from Dense RGB-D Images", F. Steinbucker, J. Strum, D. Cremers, ICCV, 2011.
*/
enum { ROTATION = 1,
- TRANSLATION = 2,
- RIGID_BODY_MOTION = 4
- };
+ TRANSLATION = 2,
+ RIGID_BODY_MOTION = 4
+ };
CV_EXPORTS bool RGBDOdometry( Mat& Rt, const Mat& initRt,
const Mat& image0, const Mat& depth0, const Mat& mask0,
const Mat& image1, const Mat& depth1, const Mat& mask1,
- const Mat& cameraMatrix, float minDepth, float maxDepth, float maxDepthDiff,
- const std::vector<int>& iterCounts, const std::vector<float>& minGradientMagnitudes,
+ const Mat& cameraMatrix, float minDepth=0.f, float maxDepth=4.f, float maxDepthDiff=0.07f,
+ const std::vector<int>& iterCounts=std::vector<int>(),
+ const std::vector<float>& minGradientMagnitudes=std::vector<float>(),
int transformType=RIGID_BODY_MOTION );
/**
CV_Assert( cameraMatrix.type() == CV_32FC1 && cameraMatrix.size() == Size(3,3) );
// other checks
- CV_Assert( !iterCounts.empty() );
- CV_Assert( minGradientMagnitudes.size() == iterCounts.size() );
+ CV_Assert( iterCounts.empty() || minGradientMagnitudes.empty() ||
+ minGradientMagnitudes.size() == iterCounts.size() );
CV_Assert( initRt.empty() || (initRt.type()==CV_64FC1 && initRt.size()==Size(4,4) ) );
+ vector<int> defaultIterCounts;
+ vector<float> defaultMinGradMagnitudes;
+ vector<int> const* iterCountsPtr = &iterCounts;
+ vector<float> const* minGradientMagnitudesPtr = &minGradientMagnitudes;
+
+ if( iterCounts.empty() || minGradientMagnitudes.empty() )
+ {
+ defaultIterCounts.resize(4);
+ defaultIterCounts[0] = 7;
+ defaultIterCounts[1] = 7;
+ defaultIterCounts[2] = 7;
+ defaultIterCounts[3] = 10;
+
+ defaultMinGradMagnitudes.resize(4);
+ defaultMinGradMagnitudes[0] = 12;
+ defaultMinGradMagnitudes[1] = 5;
+ defaultMinGradMagnitudes[2] = 3;
+ defaultMinGradMagnitudes[3] = 1;
+
+ iterCountsPtr = &defaultIterCounts;
+ minGradientMagnitudesPtr = &defaultMinGradMagnitudes;
+ }
+
preprocessDepth( depth0, depth1, validMask0, validMask1, minDepth, maxDepth );
vector<Mat> pyramidImage0, pyramidDepth0,
pyramidImage1, pyramidDepth1, pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1,
pyramidCameraMatrix;
- buildPyramids( image0, image1, depth0, depth1, cameraMatrix, sobelSize, sobelScale, minGradientMagnitudes,
+ buildPyramids( image0, image1, depth0, depth1, cameraMatrix, sobelSize, sobelScale, *minGradientMagnitudesPtr,
pyramidImage0, pyramidDepth0, pyramidImage1, pyramidDepth1,
pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1, pyramidCameraMatrix );
Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone();
Mat currRt, ksi;
- for( int level = (int)iterCounts.size() - 1; level >= 0; level-- )
+ for( int level = (int)iterCountsPtr->size() - 1; level >= 0; level-- )
{
const Mat& levelCameraMatrix = pyramidCameraMatrix[level];
Mat corresps( levelImage0.size(), levelImage0.type(), CV_32SC1 );
// Run transformation search on current level iteratively.
- for( int iter = 0; iter < iterCounts[level]; iter ++ )
+ for( int iter = 0; iter < (*iterCountsPtr)[level]; iter ++ )
{
int correspsCount = computeCorresp( levelCameraMatrix, levelCameraMatrix.inv(), resultRt.inv(DECOMP_SVD),
levelDepth0, levelDepth1, pyramidTexturedMask1[level], maxDepthDiff,