diff --git a/modules/contrib/include/opencv2/contrib/contrib.hpp b/modules/contrib/include/opencv2/contrib/contrib.hpp index 68703b28b7..93c0c51952 100644 --- a/modules/contrib/include/opencv2/contrib/contrib.hpp +++ b/modules/contrib/include/opencv2/contrib/contrib.hpp @@ -636,12 +636,12 @@ namespace cv RIGID_BODY_MOTION = 4 }; }; - CV_EXPORTS bool RGBDOdometry( cv::Mat& Rt, + CV_EXPORTS bool RGBDOdometry( cv::Mat& Rt, const Mat& initRt, const cv::Mat& image0, const cv::Mat& depth0, const cv::Mat& mask0, const cv::Mat& image1, const cv::Mat& depth1, const cv::Mat& mask1, - const cv::Mat& cameraMatrix, const std::vector& iterCounts, - const std::vector& minGradientMagnitudes, - float minDepth, float maxDepth, float maxDepthDiff, int transformType=TransformationType::RIGID_BODY_MOTION ); + const cv::Mat& cameraMatrix, float minDepth, float maxDepth, float maxDepthDiff, + const std::vector& iterCounts, const std::vector& minGradientMagnitudes, + int transformType=TransformationType::RIGID_BODY_MOTION ); } #include "opencv2/contrib/retina.hpp" diff --git a/modules/contrib/src/rgbdodometry.cpp b/modules/contrib/src/rgbdodometry.cpp index c82ead1c83..d7869eb7ee 100644 --- a/modules/contrib/src/rgbdodometry.cpp +++ b/modules/contrib/src/rgbdodometry.cpp @@ -469,12 +469,12 @@ bool computeKsi( int transformType, return solutionExist; } -bool cv::RGBDOdometry( cv::Mat& Rt, +bool cv::RGBDOdometry( cv::Mat& Rt, const Mat& initRt, const cv::Mat& image0, const cv::Mat& _depth0, const cv::Mat& validMask0, const cv::Mat& image1, const cv::Mat& _depth1, const cv::Mat& validMask1, - const cv::Mat& cameraMatrix, const std::vector& iterCounts, - const std::vector& minGradientMagnitudes, - float minDepth, float maxDepth, float maxDepthDiff, int transformType ) + const cv::Mat& cameraMatrix, float minDepth, float maxDepth, float maxDepthDiff, + const std::vector& iterCounts, const std::vector& minGradientMagnitudes, + int transformType ) { const int sobelSize = 3; const double sobelScale = 1./8; @@ -501,6 +501,7 @@ bool cv::RGBDOdometry( cv::Mat& Rt, // other checks CV_Assert( !iterCounts.empty() ); CV_Assert( minGradientMagnitudes.size() == iterCounts.size() ); + CV_Assert( initRt.empty() || (initRt.type()==CV_64FC1 && initRt.size()==Size(4,4) ) ); preprocessDepth( depth0, depth1, validMask0, validMask1, minDepth, maxDepth ); @@ -511,7 +512,8 @@ bool cv::RGBDOdometry( cv::Mat& Rt, pyramidImage0, pyramidDepth0, pyramidImage1, pyramidDepth1, pyramid_dI_dx1, pyramid_dI_dy1, pyramidTexturedMask1, pyramidCameraMatrix ); - Mat resultRt = Mat::eye(4,4,CV_64FC1), currRt, ksi; + Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone(); + Mat currRt, ksi; for( int level = iterCounts.size() - 1; level >= 0; level-- ) { const Mat& levelCameraMatrix = pyramidCameraMatrix[level]; diff --git a/samples/cpp/rgbdodometry.cpp b/samples/cpp/rgbdodometry.cpp index 3a8f1e02bc..16a0078940 100644 --- a/samples/cpp/rgbdodometry.cpp +++ b/samples/cpp/rgbdodometry.cpp @@ -150,10 +150,11 @@ int main(int argc, char** argv) const float maxDepthDiff = 0.07; //in meters tm.start(); - bool isFound = cv::RGBDOdometry( Rt, grayImage0, depthFlt0, Mat(), + bool isFound = cv::RGBDOdometry( Rt, Mat(), + grayImage0, depthFlt0, Mat(), grayImage1, depthFlt1, Mat(), - cameraMatrix, iterCounts, minGradMagnitudes, - minDepth, maxDepth, maxDepthDiff, transformationType ); + cameraMatrix, minDepth, maxDepth, maxDepthDiff, + iterCounts, minGradMagnitudes, transformationType ); tm.stop(); cout << "Rt = " << Rt << endl;