diff --git a/modules/3d/include/opencv2/3d/odometry.hpp b/modules/3d/include/opencv2/3d/odometry.hpp index f3c8282c24..e1e38f53a9 100644 --- a/modules/3d/include/opencv2/3d/odometry.hpp +++ b/modules/3d/include/opencv2/3d/odometry.hpp @@ -73,21 +73,7 @@ public: */ bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt); - /** Compute Rigid Transformation and scale between two frames so that Rt * (src * scale) = dst - * Works only on OdometryType::DEPTH and OdometryAlgoType::COMMON - * @param srcFrame src frame ("original" image) - * @param dstFrame dst frame ("rotated" image) - * @param Rt Rigid transformation, which will be calculated, in form: - * { R_11 R_12 R_13 t_1 - * R_21 R_22 R_23 t_2 - * R_31 R_32 R_33 t_3 - * 0 0 0 1 } - * @param scale scale between srcFrame and dstFrame (use scale = 1 for input) - */ - bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale); - CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const; - CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, OutputArray scale) const; CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const; class Impl; diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index ac0b184c92..6fc24d5175 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -20,8 +20,8 @@ public: virtual OdometryFrame createOdometryFrame() const = 0; virtual void prepareFrame(OdometryFrame& frame) = 0; virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) = 0; - virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const = 0; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const = 0; + virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const = 0; + virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const = 0; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const = 0; }; @@ -40,8 +40,8 @@ public: virtual OdometryFrame createOdometryFrame() const override; virtual void prepareFrame(OdometryFrame& frame) override; virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override; - virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const override; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override; + virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override; + virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; }; @@ -75,7 +75,7 @@ void OdometryICP::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype); } -bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const +bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const { Matx33f cameraMatrix; settings.getCameraMatrix(cameraMatrix); @@ -84,7 +84,7 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds settings.getIterCounts(miterCounts); for (int i = 0; i < miterCounts.size().height; i++) iterCounts.push_back(miterCounts.at(i)); - bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix, + bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix, this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), iterCounts, this->settings.getMaxTranslation(), this->settings.getMaxRotation(), settings.getSobelScale(), @@ -92,7 +92,7 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds return isCorrect; } -bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt, float& scale) const +bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const { OdometryFrame srcFrame = this->createOdometryFrame(); OdometryFrame dstFrame = this->createOdometryFrame(); @@ -101,7 +101,7 @@ bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArra prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype); - bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); + bool isCorrect = compute(srcFrame, dstFrame, Rt); return isCorrect; } @@ -123,8 +123,8 @@ public: virtual OdometryFrame createOdometryFrame() const override; virtual void prepareFrame(OdometryFrame& frame) override; virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override; - virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const override; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override; + virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override; + virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; }; @@ -154,7 +154,7 @@ void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame prepareRGBFrame(srcFrame, dstFrame, this->settings, false); } -bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const +bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const { Matx33f cameraMatrix; settings.getCameraMatrix(cameraMatrix); @@ -164,7 +164,7 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds CV_CheckTypeEQ(miterCounts.type(), CV_32S, ""); for (int i = 0; i < miterCounts.size().height; i++) iterCounts.push_back(miterCounts.at(i)); - bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix, + bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix, this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), iterCounts, this->settings.getMaxTranslation(), this->settings.getMaxRotation(), settings.getSobelScale(), @@ -172,7 +172,7 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds return isCorrect; } -bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt, float& scale) const +bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const { OdometryFrame srcFrame = this->createOdometryFrame(); OdometryFrame dstFrame = this->createOdometryFrame(); @@ -181,13 +181,13 @@ bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArra prepareRGBFrame(srcFrame, dstFrame, this->settings, false); - bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); + bool isCorrect = compute(srcFrame, dstFrame, Rt); return isCorrect; } bool OdometryRGB::compute(InputArray, InputArray, InputArray, InputArray, OutputArray) const { - CV_Error(cv::Error::StsBadFunc, "This volume does not work with depth and rgb data simultaneously"); + CV_Error(cv::Error::StsBadFunc, "This odometry does not work with depth and rgb data simultaneously"); } class OdometryRGBD : public Odometry::Impl @@ -203,8 +203,8 @@ public: virtual OdometryFrame createOdometryFrame() const override; virtual void prepareFrame(OdometryFrame& frame) override; virtual void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) override; - virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const override; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, float& scale) const override; + virtual bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const override; + virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const override; virtual bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; }; @@ -234,7 +234,7 @@ void OdometryRGBD::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFram prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype); } -bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const +bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const { Matx33f cameraMatrix; settings.getCameraMatrix(cameraMatrix); @@ -243,7 +243,7 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d settings.getIterCounts(miterCounts); for (int i = 0; i < miterCounts.size().height; i++) iterCounts.push_back(miterCounts.at(i)); - bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix, + bool isCorrect = RGBDICPOdometryImpl(Rt, Mat(), srcFrame, dstFrame, cameraMatrix, this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), iterCounts, this->settings.getMaxTranslation(), this->settings.getMaxRotation(), settings.getSobelScale(), @@ -251,7 +251,7 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d return isCorrect; } -bool OdometryRGBD::compute(InputArray, InputArray, OutputArray, float&) const +bool OdometryRGBD::compute(InputArray, InputArray, OutputArray) const { CV_Error(cv::Error::StsBadFunc, "This volume needs depth and rgb data simultaneously"); } @@ -267,8 +267,7 @@ bool OdometryRGBD::compute(InputArray _srcDepthFrame, InputArray _srcRGBFrame, dstFrame.setImage(_dstRGBFrame); prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype); - float scale = 0; - bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); + bool isCorrect = compute(srcFrame, dstFrame, Rt); return isCorrect; } @@ -346,30 +345,15 @@ void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) { - float scale = 0.f; - return this->impl->compute(srcFrame, dstFrame, Rt, scale); + return this->impl->compute(srcFrame, dstFrame, Rt); } -bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) -{ - return this->impl->compute(srcFrame, dstFrame, Rt, scale); -} bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const { - float scale = 0.f; - return this->impl->compute(srcFrame, dstFrame, Rt, scale); + return this->impl->compute(srcFrame, dstFrame, Rt); } -bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt, OutputArray _scale) const -{ - _scale.create(Size(1, 1), CV_64FC1); - Mat scaleValue = _scale.getMat(); - float scale = 1.f; - bool res = this->impl->compute(srcFrame, dstFrame, Rt, scale); - Mat(1, 1, CV_64FC1, Scalar(scale)).copyTo(scaleValue); - return res; -} bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const diff --git a/modules/3d/src/rgbd/odometry_functions.cpp b/modules/3d/src/rgbd/odometry_functions.cpp index cac6c10637..f6d9649c78 100644 --- a/modules/3d/src/rgbd/odometry_functions.cpp +++ b/modules/3d/src/rgbd/odometry_functions.cpp @@ -648,7 +648,7 @@ void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask } } -bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, +bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, const OdometryFrame srcFrame, const OdometryFrame dstFrame, const Matx33f& cameraMatrix, @@ -656,16 +656,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, double maxTranslation, double maxRotation, double sobelScale, OdometryType method, OdometryTransformType transformType, OdometryAlgoType algtype) { - if ((transformType == OdometryTransformType::RIGID_TRANSFORMATION) && - (std::abs(scale) > std::numeric_limits::epsilon())) - { - transformType = OdometryTransformType::SIM_TRANSFORMATION; - } - else - { - scale = 1.0f; - } - int transformDim = getTransformDim(transformType); const int minOverdetermScale = 20; @@ -676,14 +666,12 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, Mat resultRt = initRt.empty() ? Mat::eye(4,4,CV_64FC1) : initRt.clone(); Mat currRt, ksi; - double currScale = 1.0; Affine3f transform = Affine3f::Identity(); bool isOk = false; for(int level = (int)iterCounts.size() - 1; level >= 0; level--) { const Matx33f& levelCameraMatrix = pyramidCameraMatrix[level]; - const Matx33f& levelCameraMatrix_inv = levelCameraMatrix.inv(DECOMP_SVD); const Mat srcLevelDepth, dstLevelDepth; const Mat srcLevelImage, dstLevelImage; srcFrame.getPyramidAt(srcLevelDepth, OdometryFramePyramidType::PYR_DEPTH, level); @@ -705,32 +693,33 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, for(int iter = 0; iter < iterCounts[level]; iter ++) { Mat resultRt_inv = resultRt.inv(DECOMP_SVD); - Mat corresps_rgbd, corresps_icp, diffs_rgbd, diffs_icp; - double sigma_rgbd = 0, sigma_icp = 0; + Mat corresps_rgbd, corresps_icp, diffs_rgbd; + Mat dummy; + double sigma_rgbd = 0, dummyFloat = 0; const Mat pyramidMask; srcFrame.getPyramidAt(pyramidMask, OdometryFramePyramidType::PYR_MASK, level); - if(method != OdometryType::DEPTH)// RGB + if(method != OdometryType::DEPTH) // RGB { const Mat pyramidTexturedMask; dstFrame.getPyramidAt(pyramidTexturedMask, OdometryFramePyramidType::PYR_TEXMASK, level); - computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv, + computeCorresps(levelCameraMatrix, resultRt, srcLevelImage, srcLevelDepth, pyramidMask, dstLevelImage, dstLevelDepth, pyramidTexturedMask, maxDepthDiff, corresps_rgbd, diffs_rgbd, sigma_rgbd, OdometryType::RGB); } - if(method != OdometryType::RGB)// ICP + if(method != OdometryType::RGB) // ICP { if (algtype == OdometryAlgoType::COMMON) { const Mat pyramidNormalsMask; dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level); - computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv, + computeCorresps(levelCameraMatrix, resultRt, Mat(), srcLevelDepth, pyramidMask, Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff, - corresps_icp, diffs_icp, sigma_icp, OdometryType::DEPTH); + corresps_icp, dummy, dummyFloat, OdometryType::DEPTH); } } @@ -764,7 +753,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, if (algtype == OdometryAlgoType::COMMON) { calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals, - corresps_icp, AtA_icp, AtB_icp, currScale, transformType); + corresps_icp, AtA_icp, AtB_icp, transformType); } else { @@ -780,18 +769,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, AtB += AtB_icp; } - // workaround for bad AtA matrix - if (transformType == OdometryTransformType::SIM_TRANSFORMATION) - if (countNonZero(AtA(Range::all(), Range(6, 7))) == 0) - { - Mat tmp(6, 6, CV_64FC1, Scalar(0)); - AtA(Range(0, 6), Range(0, 6)).copyTo(tmp); - AtA = tmp; - - transformType = OdometryTransformType::RIGID_TRANSFORMATION; - transformDim = getTransformDim(transformType); - break; - } bool solutionExist = solveSystem(AtA, AtB, determinantThreshold, ksi); if (!solutionExist) @@ -800,7 +777,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, } Mat tmp61(6, 1, CV_64FC1, Scalar(0)); - double newScale = 1.0; if(transformType == OdometryTransformType::ROTATION) { ksi.copyTo(tmp61.rowRange(0,3)); @@ -811,25 +787,9 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, ksi.copyTo(tmp61.rowRange(3,6)); ksi = tmp61; } - else if (transformType == OdometryTransformType::SIM_TRANSFORMATION) - { - newScale = ksi.at(6, 0); - ksi.rowRange(0, 6).copyTo(tmp61); - ksi = tmp61; - } computeProjectiveMatrix(ksi, currRt); - //resultRt = currRt * resultRt; - - cv::Matx33d cr = currRt(cv::Rect(0, 0, 3, 3)), rr = resultRt(cv::Rect(0, 0, 3, 3)); - cv::Vec3d ct = currRt(cv::Rect(0, 3, 3, 1)), rt = resultRt(cv::Rect(0, 3, 3, 1)); - cv::Matx33d nr = cr * rr; - cv::Vec3f nt = ct + cr * rt * newScale; - Matx44d nrt = Matx44d::eye(); - nrt.get_minor<3, 3>(0, 0) = nr; - nrt.get_minor<3, 1>(0, 3) = nt; - nrt.copyTo(resultRt); - currScale *= newScale; + resultRt = currRt * resultRt; //TODO: fixit, transform is used for Fast ICP only Vec6f x(ksi); @@ -844,7 +804,6 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, _Rt.create(resultRt.size(), resultRt.type()); Mat Rt = _Rt.getMat(); resultRt.copyTo(Rt); - scale =(float)currScale; if(isOk) { Mat deltaRt; @@ -859,81 +818,84 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, return isOk; } -// Rotate dst by Rt (which is inverted in fact) to get corresponding src pixels +// Rotate dst by RtInv to get corresponding src pixels // In RGB case compute sigma and diffs too -void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, +void computeCorresps(const Matx33f& _K, const Mat& rt, const Mat& imageSrc, const Mat& depthSrc, const Mat& validMaskSrc, const Mat& imageDst, const Mat& depthDst, const Mat& selectMaskDst, float maxDepthDiff, Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method) { - CV_Assert(Rt.type() == CV_64FC1); + Mat mrtInv = rt.inv(DECOMP_SVD); + Matx44d rtInv = mrtInv; Mat corresps(depthDst.size(), CV_16SC2, Scalar::all(-1)); - Mat diffs(depthDst.size(), CV_32F, Scalar::all(-1)); + Mat diffs; + if (method == OdometryType::RGB) + diffs = Mat(depthDst.size(), CV_32F, Scalar::all(-1)); + + // src_2d = K * src_3d, src_3d = K_inv * [src_2d | z] + // - Matx33d K(_K), K_inv(_K_inv); + Matx33d K(_K); + Matx33d K_inv = K.inv(DECOMP_SVD); Rect r(0, 0, depthDst.cols, depthDst.rows); - Mat Kt = Rt(Rect(3, 0, 1, 3)).clone(); - Kt = K * Kt; - const double* Kt_ptr = Kt.ptr(); + Matx31d tinv = rtInv.get_minor<3, 1>(0, 3); + Matx31d ktinvm = K * tinv; + //const double* Kt_ptr = Kt.ptr(); + Point3d ktinv(ktinvm(0, 0), ktinvm(1, 0), ktinvm(2, 0)); AutoBuffer buf(3 * (depthDst.cols + depthDst.rows)); float* KRK_inv0_u1 = buf.data(); - float* KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_u1 + depthDst.cols; - float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depthDst.rows; - float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depthDst.cols; - float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depthDst.rows; - float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depthDst.cols; + float* KRK_inv1_v1_plus_KRK_inv2 = buf.data() + depthDst.cols; + float* KRK_inv3_u1 = buf.data() + depthDst.cols + depthDst.rows; + float* KRK_inv4_v1_plus_KRK_inv5 = buf.data() + depthDst.cols * 2 + depthDst.rows; + float* KRK_inv6_u1 = buf.data() + depthDst.cols * 2 + depthDst.rows * 2; + float* KRK_inv7_v1_plus_KRK_inv8 = buf.data() + depthDst.cols * 3 + depthDst.rows * 2; { - Mat R = Rt(Rect(0, 0, 3, 3)).clone(); + Matx33d rinv = rtInv.get_minor<3, 3>(0, 0); - Mat KRK_inv = K * R * K_inv; - const double* KRK_inv_ptr = KRK_inv.ptr(); - for (int u1 = 0; u1 < depthDst.cols; u1++) + Matx33d kriki = K * rinv * K_inv; + for (int udst = 0; udst < depthDst.cols; udst++) { - KRK_inv0_u1[u1] = (float)(KRK_inv_ptr[0] * u1); - KRK_inv3_u1[u1] = (float)(KRK_inv_ptr[3] * u1); - KRK_inv6_u1[u1] = (float)(KRK_inv_ptr[6] * u1); + KRK_inv0_u1[udst] = (float)(kriki(0, 0) * udst); + KRK_inv3_u1[udst] = (float)(kriki(1, 0) * udst); + KRK_inv6_u1[udst] = (float)(kriki(2, 0) * udst); } - for (int v1 = 0; v1 < depthDst.rows; v1++) + for (int vdst = 0; vdst < depthDst.rows; vdst++) { - KRK_inv1_v1_plus_KRK_inv2[v1] = (float)(KRK_inv_ptr[1] * v1 + KRK_inv_ptr[2]); - KRK_inv4_v1_plus_KRK_inv5[v1] = (float)(KRK_inv_ptr[4] * v1 + KRK_inv_ptr[5]); - KRK_inv7_v1_plus_KRK_inv8[v1] = (float)(KRK_inv_ptr[7] * v1 + KRK_inv_ptr[8]); + KRK_inv1_v1_plus_KRK_inv2[vdst] = (float)(kriki(0, 1) * vdst + kriki(0, 2)); + KRK_inv4_v1_plus_KRK_inv5[vdst] = (float)(kriki(1, 1) * vdst + kriki(1, 2)); + KRK_inv7_v1_plus_KRK_inv8[vdst] = (float)(kriki(2, 1) * vdst + kriki(2, 2)); } } double sigma = 0; int correspCount = 0; - for (int v1 = 0; v1 < depthDst.rows; v1++) + for (int vdst = 0; vdst < depthDst.rows; vdst++) { - const float* depth1_row = depthDst.ptr(v1); - const uchar* mask1_row = selectMaskDst.ptr(v1); - for (int u1 = 0; u1 < depthDst.cols; u1++) + const float* depthDst_row = depthDst.ptr(vdst); + const uchar* maskDst_row = selectMaskDst.ptr(vdst); + for (int udst = 0; udst < depthDst.cols; udst++) { - float d1 = depth1_row[u1]; - if (mask1_row[u1] && !cvIsNaN(d1)) + float ddst = depthDst_row[udst]; + + if (maskDst_row[udst] && !cvIsNaN(ddst)) { - //CV_DbgAssert(!cvIsNaN(d1)); - float transformed_d1 = static_cast(d1 * (KRK_inv6_u1[u1] + KRK_inv7_v1_plus_KRK_inv8[v1]) + - Kt_ptr[2]); + float transformed_ddst = static_cast(ddst * (KRK_inv6_u1[udst] + KRK_inv7_v1_plus_KRK_inv8[vdst]) + ktinv.z); - if (transformed_d1 > 0) + if (transformed_ddst > 0) { - float transformed_d1_inv = 1.f / transformed_d1; - int u0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv0_u1[u1] + KRK_inv1_v1_plus_KRK_inv2[v1]) + - Kt_ptr[0])); - int v0 = cvRound(transformed_d1_inv * (d1 * (KRK_inv3_u1[u1] + KRK_inv4_v1_plus_KRK_inv5[v1]) + - Kt_ptr[1])); - if (r.contains(Point(u0, v0))) + float transformed_ddst_inv = 1.f / transformed_ddst; + int usrc = cvRound(transformed_ddst_inv * (ddst * (KRK_inv0_u1[udst] + KRK_inv1_v1_plus_KRK_inv2[vdst]) + ktinv.x)); + int vsrc = cvRound(transformed_ddst_inv * (ddst * (KRK_inv3_u1[udst] + KRK_inv4_v1_plus_KRK_inv5[vdst]) + ktinv.y)); + if (r.contains(Point(usrc, vsrc))) { - float d0 = depthSrc.at(v0, u0); - if (validMaskSrc.at(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff) + float dsrc = depthSrc.at(vsrc, usrc); + if (validMaskSrc.at(vsrc, usrc) && std::abs(transformed_ddst - dsrc) <= maxDepthDiff) { - CV_DbgAssert(!cvIsNaN(d0)); - Vec2s& c = corresps.at(v0, u0); - float& d = diffs.at(v0, u0); + CV_DbgAssert(!cvIsNaN(dsrc)); + Vec2s& c = corresps.at(vsrc, usrc); float diff = 0; if (c[0] != -1) { @@ -941,24 +903,27 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, int exist_u1 = c[0], exist_v1 = c[1]; float exist_d1 = (float)(depthDst.at(exist_v1, exist_u1) * - (KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + Kt_ptr[2]); + (KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + ktinv.z); - if (transformed_d1 > exist_d1) + if (transformed_ddst > exist_d1) continue; if (method == OdometryType::RGB) - diff = static_cast(static_cast(imageSrc.at(v0, u0)) - - static_cast(imageDst.at(v1, u1))); + diff = static_cast(static_cast(imageSrc.at(vsrc, usrc)) - + static_cast(imageDst.at(vdst, udst))); } else { if (method == OdometryType::RGB) - diff = static_cast(static_cast(imageSrc.at(v0, u0)) - - static_cast(imageDst.at(v1, u1))); + diff = static_cast(static_cast(imageSrc.at(vsrc, usrc)) - + static_cast(imageDst.at(vdst, udst))); correspCount++; } - c = Vec2s((short)u1, (short)v1); - d = diff; - sigma += diff * diff; + c = Vec2s((short)udst, (short)vdst); + if (method == OdometryType::RGB) + { + diffs.at(vsrc, usrc) = diff; + sigma += diff * diff; + } } } } @@ -969,20 +934,26 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, _sigma = std::sqrt(sigma / double(correspCount)); _corresps.create(correspCount, 1, CV_32SC4); - _diffs.create(correspCount, 1, CV_32F); Vec4i* corresps_ptr = _corresps.ptr(); - float* diffs_ptr = _diffs.ptr(); - for (int v0 = 0, i = 0; v0 < corresps.rows; v0++) + float* diffs_ptr; + if (method == OdometryType::RGB) { - const Vec2s* corresps_row = corresps.ptr(v0); - const float* diffs_row = diffs.ptr(v0); - for (int u0 = 0; u0 < corresps.cols; u0++) + _diffs.create(correspCount, 1, CV_32F); + diffs_ptr = _diffs.ptr(); + } + for (int vsrc = 0, i = 0; vsrc < corresps.rows; vsrc++) + { + const Vec2s* corresps_row = corresps.ptr(vsrc); + const float* diffs_row; + if (method == OdometryType::RGB) + diffs_row = diffs.ptr(vsrc); + for (int usrc = 0; usrc < corresps.cols; usrc++) { - const Vec2s& c = corresps_row[u0]; - const float& d = diffs_row[u0]; + const Vec2s& c = corresps_row[usrc]; + const float& d = diffs_row[usrc]; if (c[0] != -1) { - corresps_ptr[i] = Vec4i(u0, v0, c[0], c[1]); + corresps_ptr[i] = Vec4i(usrc, vsrc, c[0], c[1]); if (method == OdometryType::RGB) diffs_ptr[i] = d; i++; @@ -1003,7 +974,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, double* AtB_ptr = AtB.ptr(); CV_Assert(Rt.type() == CV_64FC1); - const double* Rt_ptr = Rt.ptr(); + Affine3d rtmat(Rt); const float* diffs_ptr = _diffs.ptr(); const Vec4i* corresps_ptr = corresps.ptr(); @@ -1024,10 +995,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, double w_sobelScale = w * sobelScaleIn; const Vec4f& p0 = cloud0.at(v0, u0); - Point3f tp0; - tp0.x = (float)(p0[0] * Rt_ptr[0] + p0[1] * Rt_ptr[1] + p0[2] * Rt_ptr[2] + Rt_ptr[3]); - tp0.y = (float)(p0[0] * Rt_ptr[4] + p0[1] * Rt_ptr[5] + p0[2] * Rt_ptr[6] + Rt_ptr[7]); - tp0.z = (float)(p0[0] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]); + Point3f tp0 = rtmat * Point3f(p0[0], p0[1], p0[2]); rgbdCoeffsFunc(transformType, A_ptr, @@ -1055,7 +1023,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, const Mat& cloud1, const Mat& normals1, const Mat& corresps, - Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType) + Mat& AtA, Mat& AtB, OdometryTransformType transformType) { int transformDim = getTransformDim(transformType); AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0)); @@ -1084,9 +1052,9 @@ void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, const Vec4f& p0 = cloud0.at(v0, u0); Point3f tp0; - tp0.x = (float)(p0[0] * scale * Rt_ptr[0] + p0[1] * scale * Rt_ptr[1] + p0[2] * scale * Rt_ptr[2] + Rt_ptr[3]); - tp0.y = (float)(p0[0] * scale * Rt_ptr[4] + p0[1] * scale * Rt_ptr[5] + p0[2] * scale * Rt_ptr[6] + Rt_ptr[7]); - tp0.z = (float)(p0[0] * scale * Rt_ptr[8] + p0[1] * scale * Rt_ptr[9] + p0[2] * scale * Rt_ptr[10] + Rt_ptr[11]); + tp0.x = (float)(p0[0] * Rt_ptr[0] + p0[1] * Rt_ptr[1] + p0[2] * Rt_ptr[2] + Rt_ptr[3]); + tp0.y = (float)(p0[0] * Rt_ptr[4] + p0[1] * Rt_ptr[5] + p0[2] * Rt_ptr[6] + Rt_ptr[7]); + tp0.z = (float)(p0[0] * Rt_ptr[8] + p0[1] * Rt_ptr[9] + p0[2] * Rt_ptr[10] + Rt_ptr[11]); Vec4f n1 = normals1.at(v1, u1); Vec4f _v = cloud1.at(v1, u1); diff --git a/modules/3d/src/rgbd/odometry_functions.hpp b/modules/3d/src/rgbd/odometry_functions.hpp index 8b190996c2..aa1530ed9c 100644 --- a/modules/3d/src/rgbd/odometry_functions.hpp +++ b/modules/3d/src/rgbd/odometry_functions.hpp @@ -12,16 +12,14 @@ namespace cv { enum class OdometryTransformType { - // rotation, translation, rotation+translation, rotation*scale+translation - ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4, SIM_TRANSFORMATION = 8 + // rotation, translation, rotation+translation + ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4 }; static inline int getTransformDim(OdometryTransformType transformType) { switch(transformType) { - case OdometryTransformType::SIM_TRANSFORMATION: - return 7; case OdometryTransformType::RIGID_TRANSFORMATION: return 6; case OdometryTransformType::ROTATION: @@ -40,6 +38,7 @@ void checkImage(InputArray image) if (image.type() != CV_8UC1) CV_Error(Error::StsBadSize, "Image type has to be CV_8UC1."); } + static inline void checkDepth(InputArray depth, const Size& imageSize) { @@ -72,28 +71,9 @@ void checkNormals(InputArray normals, const Size& depthSize) CV_Error(Error::StsBadSize, "Normals type has to be CV_32FC3."); } -static inline -void calcRgbdScaleEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) -{ - double invz = 1. / p3d.z, - v0 = dIdx * fx * invz, - v1 = dIdy * fy * invz, - v2 = -(v0 * p3d.x + v1 * p3d.y) * invz; - Point3d v(v0, v1, v2); - Point3d pxv = p3d.cross(v); - - C[0] = pxv.x; - C[1] = pxv.y; - C[2] = pxv.z; - C[3] = v0; - C[4] = v1; - C[5] = v2; - //TODO: fixit - C[6] = 0; -} static inline -void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) +Vec6d calcRgbdEquationCoeffs(double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) { double invz = 1. / p3d.z, v0 = dIdx * fx * invz, @@ -102,16 +82,11 @@ void calcRgbdEquationCoeffs(double* C, double dIdx, double dIdy, const Point3f& Point3d v(v0, v1, v2); Point3d pxv = p3d.cross(v); - C[0] = pxv.x; - C[1] = pxv.y; - C[2] = pxv.z; - C[3] = v0; - C[4] = v1; - C[5] = v2; + return Vec6d(pxv.x, pxv.y, pxv.z, v0, v1, v2); } static inline -void calcRgbdEquationCoeffsRotation(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) +Vec3d calcRgbdEquationCoeffsRotation(double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) { double invz = 1. / p3d.z, v0 = dIdx * fx * invz, @@ -121,117 +96,103 @@ void calcRgbdEquationCoeffsRotation(double* C, double dIdx, double dIdy, const P Point3d v(v0, v1, v2); Point3d pxv = p3d.cross(v); - C[0] = pxv.x; - C[1] = pxv.y; - C[2] = pxv.z; + return Vec3d(pxv); } static inline -void calcRgbdEquationCoeffsTranslation(double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) +Vec3d calcRgbdEquationCoeffsTranslation(double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) { double invz = 1. / p3d.z, v0 = dIdx * fx * invz, v1 = dIdy * fy * invz, v2 = -(v0 * p3d.x + v1 * p3d.y) * invz; - C[0] = v0; - C[1] = v1; - C[2] = v2; + + return Vec3d(v0, v1, v2); } static inline void rgbdCoeffsFunc(OdometryTransformType transformType, double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) { + int dim = getTransformDim(transformType); + Vec6d ret; switch(transformType) { - case OdometryTransformType::SIM_TRANSFORMATION: - calcRgbdScaleEquationCoeffs(C, dIdx, dIdy, p3d, fx, fy); - break; case OdometryTransformType::RIGID_TRANSFORMATION: - calcRgbdEquationCoeffs(C, dIdx, dIdy, p3d, fx, fy); + { + ret = calcRgbdEquationCoeffs(dIdx, dIdy, p3d, fx, fy); break; + } case OdometryTransformType::ROTATION: - calcRgbdEquationCoeffsRotation(C, dIdx, dIdy, p3d, fx, fy); + { + Vec3d r = calcRgbdEquationCoeffsRotation(dIdx, dIdy, p3d, fx, fy); + ret = Vec6d(r[0], r[1], r[2], 0, 0, 0); break; + } case OdometryTransformType::TRANSLATION: - calcRgbdEquationCoeffsTranslation(C, dIdx, dIdy, p3d, fx, fy); + { + Vec3d r = calcRgbdEquationCoeffsTranslation(dIdx, dIdy, p3d, fx, fy); + ret = Vec6d(r[0], r[1], r[2], 0, 0, 0); break; + } default: CV_Error(Error::StsBadArg, "Incorrect transformation type"); } + for (int i = 0; i < dim; i++) + C[i] = ret[i]; } -static inline -void calcICPScaleEquationCoeffs(double* C, const Point3f& p0, const Point3f& p1, const Vec3f& n1) -{ - Point3d pxv = p0.cross(Point3d(n1)); - - C[0] = pxv.x; - C[1] = pxv.y; - C[2] = pxv.z; - C[3] = n1[0]; - C[4] = n1[1]; - C[5] = n1[2]; - double diff = n1.dot(p1 - p0); - //TODO: fixit - if (diff < DBL_EPSILON || abs(diff) > 1000000) - C[6] = 0; - else - //C[6] = n1.dot(p1-p0); - C[6] = -n1.dot(p0); - //C[6] = n1.dot((p0 - currentTranslation)/currentScale); -} static inline -void calcICPEquationCoeffs(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1) +Vec6d calcICPEquationCoeffs(const Point3f& psrc, const Vec3f& ndst) { - Point3d pxv = p0.cross(Point3d(n1)); - - C[0] = pxv.x; - C[1] = pxv.y; - C[2] = pxv.z; - C[3] = n1[0]; - C[4] = n1[1]; - C[5] = n1[2]; + Point3d pxv = psrc.cross(Point3d(ndst)); + + return Vec6d(pxv.x, pxv.y, pxv.z, ndst[0], ndst[1], ndst[2]); } static inline -void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1) +Vec3d calcICPEquationCoeffsRotation(const Point3f& psrc, const Vec3f& ndst) { - Point3d pxv = p0.cross(Point3d(n1)); + Point3d pxv = psrc.cross(Point3d(ndst)); - C[0] = pxv.x; - C[1] = pxv.y; - C[2] = pxv.z; + return Vec3d(pxv); } static inline -void calcICPEquationCoeffsTranslation(double* C, const Point3f& /*p0*/, const Point3f& /*p1*/, const Vec3f& n1) +Vec3d calcICPEquationCoeffsTranslation( const Point3f& /*p0*/, const Vec3f& ndst) { - C[0] = n1[0]; - C[1] = n1[1]; - C[2] = n1[2]; + return Vec3d(ndst); } static inline -void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3f& p0, const Point3f& p1, const Vec3f& n1) +void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1) { + int dim = getTransformDim(transformType); + Vec6d ret; switch(transformType) { - case OdometryTransformType::SIM_TRANSFORMATION: - calcICPScaleEquationCoeffs(C, p0, p1, n1); - break; case OdometryTransformType::RIGID_TRANSFORMATION: - calcICPEquationCoeffs(C, p0, p1, n1);; + { + ret = calcICPEquationCoeffs(p0, n1); break; + } case OdometryTransformType::ROTATION: - calcICPEquationCoeffsRotation(C, p0, p1, n1);; + { + Vec3d r = calcICPEquationCoeffsRotation(p0, n1); + ret = Vec6d(r[0], r[1], r[2], 0, 0, 0); break; + } case OdometryTransformType::TRANSLATION: - calcICPEquationCoeffsTranslation(C, p0, p1, n1);; + { + Vec3d r = calcICPEquationCoeffsTranslation(p0, n1); + ret = Vec6d(r[0], r[1], r[2], 0, 0, 0); break; + } default: CV_Error(Error::StsBadArg, "Incorrect transformation type"); } + for (int i = 0; i < dim; i++) + C[i] = ret[i]; } void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype); @@ -275,8 +236,7 @@ void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask InputOutputArrayOfArrays /*std::vector&*/ pyramidNormalsMask); -// scale = 0, if not needs scale; otherwise scale = 1; -bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, +bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, const OdometryFrame srcFrame, const OdometryFrame dstFrame, const Matx33f& cameraMatrix, @@ -284,7 +244,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, double maxTranslation, double maxRotation, double sobelScale, OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype); -void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, +void computeCorresps(const Matx33f& _K, const Mat& Rt, const Mat& image0, const Mat& depth0, const Mat& validMask0, const Mat& image1, const Mat& depth1, const Mat& selectMask1, float maxDepthDiff, Mat& _corresps, Mat& _diffs, double& _sigma, OdometryType method); @@ -298,7 +258,7 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, const Mat& cloud1, const Mat& normals1, const Mat& corresps, - Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType); + Mat& AtA, Mat& AtB, OdometryTransformType transformType); void calcICPLsmMatricesFast(Matx33f cameraMatrix, const Mat& oldPts, const Mat& oldNrm, const Mat& newPts, const Mat& newNrm, cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b); diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index d8f81fa13d..498a75d421 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -156,7 +156,6 @@ public: OdometryAlgoType algtype; double maxError1; double maxError5; - bool testScale; double idError; }; @@ -251,17 +250,13 @@ void OdometryTest::run() odf.setImage(image); odf.setDepth(depth); Mat calcRt; - float scale = 1.0f; // 1. Try to find Rt between the same frame (try masks also). Mat mask(image.size(), CV_8UC1, Scalar(255)); odometry.prepareFrame(odf); bool isComputed; - if (testScale) - isComputed = odometry.compute(odf, odf, calcRt, scale); - else - isComputed = odometry.compute(odf, odf, calcRt); + isComputed = odometry.compute(odf, odf, calcRt); if(!isComputed) { @@ -290,8 +285,6 @@ void OdometryTest::run() Mat warpedImage, warpedDepth, scaledDepth; - float test_scale = 1.03f; - scaledDepth = testScale ? depth * test_scale : depth; warpFrame(image, scaledDepth, rvec, tvec, K, warpedImage, warpedDepth); dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping @@ -299,18 +292,13 @@ void OdometryTest::run() OdometryFrame odfSrc = odometry.createOdometryFrame(); OdometryFrame odfDst = odometry.createOdometryFrame(); - float scale_error = 0.05f; - odfSrc.setImage(image); odfSrc.setDepth(depth); odfDst.setImage(warpedImage); odfDst.setDepth(warpedDepth); odometry.prepareFrames(odfSrc, odfDst); - if (testScale) - isComputed = odometry.compute(odfSrc, odfDst, calcRt, scale); - else - isComputed = odometry.compute(odfSrc, odfDst, calcRt); + isComputed = odometry.compute(odfSrc, odfDst, calcRt); if (!isComputed) { @@ -334,8 +322,6 @@ void OdometryTest::run() // compare rotation double possibleError = algtype == OdometryAlgoType::COMMON ? 0.11f : 0.015f; - if (testScale) - possibleError = 0.2f; Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec)); Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec)); @@ -344,15 +330,14 @@ void OdometryTest::run() double rdiffnorm = cv::norm(diff.rvec()); double tdiffnorm = cv::norm(diff.translation()); - if (rdiffnorm < possibleError && tdiffnorm < possibleError && abs(scale - test_scale) < scale_error) + if (rdiffnorm < possibleError && tdiffnorm < possibleError) better_1time_count++; - if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError && abs(scale - test_scale) < scale_error) + if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError) better_5times_count++; CV_LOG_INFO(NULL, "Iter " << iter); CV_LOG_INFO(NULL, "rdiff: " << Vec3f(diff.rvec()) << "; rdiffnorm: " << rdiffnorm); CV_LOG_INFO(NULL, "tdiff: " << Vec3f(diff.translation()) << "; tdiffnorm: " << tdiffnorm); - CV_LOG_INFO(NULL, "test_scale: " << test_scale << "; scale: " << scale); CV_LOG_INFO(NULL, "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count); } @@ -423,12 +408,6 @@ TEST(RGBD_Odometry_ICP, algorithmic) test.run(); } -TEST(RGBD_Odometry_ICP_Scale, algorithmic) -{ - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.65, 0.0, true); - test.run(); -} - TEST(RGBD_Odometry_RgbdICP, algorithmic) { OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99); @@ -437,7 +416,7 @@ TEST(RGBD_Odometry_RgbdICP, algorithmic) TEST(RGBD_Odometry_FastICP, algorithmic) { - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON); + OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON); test.run(); } @@ -462,7 +441,7 @@ TEST(RGBD_Odometry_RgbdICP, UMats) TEST(RGBD_Odometry_FastICP, UMats) { - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON); + OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON); test.checkUMats(); }