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<int>& iterCounts,
- const std::vector<float>& 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<int>& iterCounts, const std::vector<float>& minGradientMagnitudes,
+ int transformType=TransformationType::RIGID_BODY_MOTION );
}
#include "opencv2/contrib/retina.hpp"
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<int>& iterCounts,
- const std::vector<float>& minGradientMagnitudes,
- float minDepth, float maxDepth, float maxDepthDiff, int transformType )
+ const cv::Mat& cameraMatrix, float minDepth, float maxDepth, float maxDepthDiff,
+ const std::vector<int>& iterCounts, const std::vector<float>& minGradientMagnitudes,
+ int transformType )
{
const int sobelSize = 3;
const double sobelScale = 1./8;
// 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 );
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];
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;