From df490c6399ba4bbc24c3d430dee6bb776e6d1946 Mon Sep 17 00:00:00 2001 From: Saratovtsev Date: Tue, 8 Feb 2022 18:14:32 +0300 Subject: [PATCH 01/22] moving code from ICP+Scale --- modules/3d/include/opencv2/3d/odometry.hpp | 24 ++- modules/3d/misc/python/test/test_odometry.py | 31 +++ modules/3d/src/rgbd/odometry.cpp | 93 ++++---- modules/3d/src/rgbd/odometry_functions.cpp | 205 ++++++++++-------- modules/3d/src/rgbd/odometry_functions.hpp | 210 ++++++++++++++----- modules/3d/test/test_odometry.cpp | 55 +++-- samples/python/odometry.py | 17 +- 7 files changed, 432 insertions(+), 203 deletions(-) diff --git a/modules/3d/include/opencv2/3d/odometry.hpp b/modules/3d/include/opencv2/3d/odometry.hpp index 8bd112c2b2..f3c8282c24 100644 --- a/modules/3d/include/opencv2/3d/odometry.hpp +++ b/modules/3d/include/opencv2/3d/odometry.hpp @@ -16,7 +16,7 @@ namespace cv /** These constants are used to set a type of data which odometry will use * @param DEPTH only depth data * @param RGB only rgb image -* @param RGB_DEPTH only depth and rgb data simultaneously +* @param RGB_DEPTH depth and rgb data simultaneously */ enum OdometryType { @@ -26,8 +26,8 @@ enum OdometryType }; /** These constants are used to set the speed and accuracy of odometry -* @param COMMON only accurate but not so fast -* @param FAST only less accurate but faster +* @param COMMON accurate but not so fast +* @param FAST less accurate but faster */ enum class OdometryAlgoType { @@ -62,9 +62,9 @@ public: */ void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame); - /** Prepare frame for odometry calculation + /** Compute Rigid Transformation between two frames so that Rt * src = dst * @param srcFrame src frame ("original" image) - * @param dstFrame dsr frame ("rotated" 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 @@ -73,7 +73,21 @@ 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/misc/python/test/test_odometry.py b/modules/3d/misc/python/test/test_odometry.py index cc16725d20..b961f361fd 100644 --- a/modules/3d/misc/python/test/test_odometry.py +++ b/modules/3d/misc/python/test/test_odometry.py @@ -108,5 +108,36 @@ class odometry_test(NewOpenCVTests): self.assertLessEqual(res, eps) self.assertTrue(isCorrect) + def test_OdometryScale(self): + depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) + radian = np.radians(1) + Rt_warp = np.array( + [[np.cos(radian), -np.sin(radian), 0], + [np.sin(radian), np.cos(radian), 0], + [0, 0, 1]], dtype=np.float32 + ) + Rt_curr = np.array( + [[np.cos(radian), -np.sin(radian), 0, 0], + [np.sin(radian), np.cos(radian), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]], dtype=np.float32 + ) + Rt_res = np.zeros((4, 4)) + scale = 1.01 + scale_res = np.zeros((1, 1)) + + odometry = cv.Odometry() + warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) + + isCorrect = odometry.compute(depth, warped_depth*scale, Rt_res, scale_res) + Rt_diff = np.absolute(Rt_curr - Rt_res).sum() + scale_diff = np.absolute(scale - scale_res[0][0]) + + Rt_eps = 0.2 + scale_eps = 0.1 + self.assertLessEqual(Rt_diff, Rt_eps) + self.assertLessEqual(scale_diff, scale_eps) + self.assertTrue(isCorrect) + if __name__ == '__main__': NewOpenCVTests.bootstrap() diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index 34ef6ed57f..7afe1eca5a 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) const = 0; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const = 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(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) const override; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const 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(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) const +bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const { Matx33f cameraMatrix; settings.getCameraMatrix(cameraMatrix); @@ -84,15 +84,15 @@ 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, Mat(), srcFrame, dstFrame, cameraMatrix, - this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), - iterCounts, this->settings.getMaxTranslation(), - this->settings.getMaxRotation(), settings.getSobelScale(), - OdometryType::DEPTH, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); + bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix, + this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), + iterCounts, this->settings.getMaxTranslation(), + this->settings.getMaxRotation(), settings.getSobelScale(), + OdometryType::DEPTH, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); return isCorrect; } -bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const +bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt, float& scale) const { OdometryFrame srcFrame = this->createOdometryFrame(); OdometryFrame dstFrame = this->createOdometryFrame(); @@ -101,13 +101,13 @@ bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArra prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype); - bool isCorrect = compute(srcFrame, dstFrame, Rt); + bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); return isCorrect; } bool OdometryICP::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 OdometryRGB : public Odometry::Impl @@ -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) const override; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const 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(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) const +bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const { Matx33f cameraMatrix; settings.getCameraMatrix(cameraMatrix); @@ -164,14 +164,15 @@ 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, Mat(), srcFrame, dstFrame, cameraMatrix, - this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), - iterCounts, this->settings.getMaxTranslation(), - this->settings.getMaxRotation(), settings.getSobelScale(), - OdometryType::RGB, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); + bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix, + this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), + iterCounts, this->settings.getMaxTranslation(), + this->settings.getMaxRotation(), settings.getSobelScale(), + OdometryType::RGB, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); return isCorrect; } -bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const + +bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt, float& scale) const { OdometryFrame srcFrame = this->createOdometryFrame(); OdometryFrame dstFrame = this->createOdometryFrame(); @@ -180,7 +181,7 @@ bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArra prepareRGBFrame(srcFrame, dstFrame, this->settings, false); - bool isCorrect = compute(srcFrame, dstFrame, Rt); + bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); return isCorrect; } @@ -202,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) const override; - virtual bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const 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(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; }; @@ -233,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) const +bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt, float& scale) const { Matx33f cameraMatrix; settings.getCameraMatrix(cameraMatrix); @@ -242,21 +243,21 @@ 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, Mat(), srcFrame, dstFrame, cameraMatrix, - this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), - iterCounts, this->settings.getMaxTranslation(), - this->settings.getMaxRotation(), settings.getSobelScale(), - OdometryType::RGB_DEPTH, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); + bool isCorrect = RGBDICPOdometryImpl(Rt, scale, Mat(), srcFrame, dstFrame, cameraMatrix, + this->settings.getMaxDepthDiff(), this->settings.getAngleThreshold(), + iterCounts, this->settings.getMaxTranslation(), + this->settings.getMaxRotation(), settings.getSobelScale(), + OdometryType::RGB_DEPTH, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); return isCorrect; } -bool OdometryRGBD::compute(InputArray, InputArray, OutputArray) const +bool OdometryRGBD::compute(InputArray, InputArray, OutputArray, float&) const { CV_Error(cv::Error::StsBadFunc, "This volume needs depth and rgb data simultaneously"); } bool OdometryRGBD::compute(InputArray _srcDepthFrame, InputArray _srcRGBFrame, - InputArray _dstDepthFrame, InputArray _dstRGBFrame, OutputArray Rt) const + InputArray _dstDepthFrame, InputArray _dstRGBFrame, OutputArray Rt) const { OdometryFrame srcFrame = this->createOdometryFrame(); OdometryFrame dstFrame = this->createOdometryFrame(); @@ -266,8 +267,8 @@ bool OdometryRGBD::compute(InputArray _srcDepthFrame, InputArray _srcRGBFrame, dstFrame.setImage(_dstRGBFrame); prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype); - - bool isCorrect = compute(srcFrame, dstFrame, Rt); + float scale = 0; + bool isCorrect = compute(srcFrame, dstFrame, Rt, scale); return isCorrect; } @@ -345,15 +346,31 @@ void Odometry::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) { - //this->prepareFrames(srcFrame, dstFrame); - return this->impl->compute(srcFrame, dstFrame, Rt); + float scale = 0.f; + return this->impl->compute(srcFrame, dstFrame, Rt, scale); +} + +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 29abe7e146..cac6c10637 100644 --- a/modules/3d/src/rgbd/odometry_functions.cpp +++ b/modules/3d/src/rgbd/odometry_functions.cpp @@ -359,8 +359,8 @@ void preparePyramidImage(InputArray image, InputOutputArrayOfArrays pyramidImage template void preparePyramidMask(InputArray mask, InputArrayOfArrays pyramidDepth, float minDepth, float maxDepth, - InputArrayOfArrays pyramidNormal, - InputOutputArrayOfArrays pyramidMask) + InputArrayOfArrays pyramidNormal, + InputOutputArrayOfArrays pyramidMask) { minDepth = std::max(0.f, minDepth); @@ -492,8 +492,8 @@ void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputO } void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy, - InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale) + InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart, + InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale) { size_t didxLevels = pyramid_dI_dx.size(-1).width; size_t texLevels = pyramidTexturedMask.size(-1).width; @@ -606,7 +606,7 @@ void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, } void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays /*std::vector&*/ pyramidNormalsMask) + InputOutputArrayOfArrays /*std::vector&*/ pyramidNormalsMask) { size_t maskLevels = pyramidMask.size(-1).width; size_t norMaskLevels = pyramidNormalsMask.size(-1).width; @@ -648,39 +648,26 @@ void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask } } - -bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, +bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, const OdometryFrame srcFrame, const OdometryFrame dstFrame, const Matx33f& cameraMatrix, float maxDepthDiff, float angleThreshold, const std::vector& iterCounts, double maxTranslation, double maxRotation, double sobelScale, - OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype) + OdometryType method, OdometryTransformType transformType, OdometryAlgoType algtype) { - int transformDim = -1; - CalcRgbdEquationCoeffsPtr rgbdEquationFuncPtr = 0; - CalcICPEquationCoeffsPtr icpEquationFuncPtr = 0; - switch(transfromType) + if ((transformType == OdometryTransformType::RIGID_TRANSFORMATION) && + (std::abs(scale) > std::numeric_limits::epsilon())) + { + transformType = OdometryTransformType::SIM_TRANSFORMATION; + } + else { - case OdometryTransformType::RIGID_TRANSFORMATION: - transformDim = 6; - rgbdEquationFuncPtr = calcRgbdEquationCoeffs; - icpEquationFuncPtr = calcICPEquationCoeffs; - break; - case OdometryTransformType::ROTATION: - transformDim = 3; - rgbdEquationFuncPtr = calcRgbdEquationCoeffsRotation; - icpEquationFuncPtr = calcICPEquationCoeffsRotation; - break; - case OdometryTransformType::TRANSLATION: - transformDim = 3; - rgbdEquationFuncPtr = calcRgbdEquationCoeffsTranslation; - icpEquationFuncPtr = calcICPEquationCoeffsTranslation; - break; - default: - CV_Error(Error::StsBadArg, "Incorrect transformation type"); + scale = 1.0f; } + int transformDim = getTransformDim(transformType); + const int minOverdetermScale = 20; const int minCorrespsCount = minOverdetermScale * transformDim; @@ -689,6 +676,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, 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; @@ -740,9 +728,9 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, const Mat pyramidNormalsMask; dstFrame.getPyramidAt(pyramidNormalsMask, OdometryFramePyramidType::PYR_NORMMASK, level); computeCorresps(levelCameraMatrix, levelCameraMatrix_inv, resultRt_inv, - Mat(), srcLevelDepth, pyramidMask, - Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff, - corresps_icp, diffs_icp, sigma_icp, OdometryType::DEPTH); + Mat(), srcLevelDepth, pyramidMask, + Mat(), dstLevelDepth, pyramidNormalsMask, maxDepthDiff, + corresps_icp, diffs_icp, sigma_icp, OdometryType::DEPTH); } } @@ -763,7 +751,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, dstFrame.getPyramidAt(dstPyrIdy, OdometryFramePyramidType::PYR_DIY, level); calcRgbdLsmMatrices(srcPyrCloud, resultRt, dstPyrIdx, dstPyrIdy, corresps_rgbd, diffs_rgbd, sigma_rgbd, fx, fy, sobelScale, - AtA_rgbd, AtB_rgbd, rgbdEquationFuncPtr, transformDim); + AtA_rgbd, AtB_rgbd, transformType); AtA += AtA_rgbd; AtB += AtB_rgbd; } @@ -776,7 +764,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, if (algtype == OdometryAlgoType::COMMON) { calcICPLsmMatrices(srcPyrCloud, resultRt, dstPyrCloud, dstPyrNormals, - corresps_icp, AtA_icp, AtB_icp, icpEquationFuncPtr, transformDim); + corresps_icp, AtA_icp, AtB_icp, currScale, transformType); } else { @@ -792,33 +780,63 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, 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) { break; } - if(transfromType == OdometryTransformType::ROTATION) + + Mat tmp61(6, 1, CV_64FC1, Scalar(0)); + double newScale = 1.0; + if(transformType == OdometryTransformType::ROTATION) + { + ksi.copyTo(tmp61.rowRange(0,3)); + ksi = tmp61; + } + else if(transformType == OdometryTransformType::TRANSLATION) { - Mat tmp(6, 1, CV_64FC1, Scalar(0)); - ksi.copyTo(tmp.rowRange(0,3)); - ksi = tmp; + ksi.copyTo(tmp61.rowRange(3,6)); + ksi = tmp61; } - else if(transfromType == OdometryTransformType::TRANSLATION) + else if (transformType == OdometryTransformType::SIM_TRANSFORMATION) { - Mat tmp(6, 1, CV_64FC1, Scalar(0)); - ksi.copyTo(tmp.rowRange(3,6)); - ksi = tmp; + newScale = ksi.at(6, 0); + ksi.rowRange(0, 6).copyTo(tmp61); + ksi = tmp61; } computeProjectiveMatrix(ksi, currRt); - resultRt = currRt * resultRt; + //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; + + //TODO: fixit, transform is used for Fast ICP only Vec6f x(ksi); Affine3f tinc(Vec3f(x.val), Vec3f(x.val + 3)); transform = tinc * transform; isOk = true; - } } @@ -826,7 +844,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, _Rt.create(resultRt.size(), resultRt.type()); Mat Rt = _Rt.getMat(); resultRt.copyTo(Rt); - + scale =(float)currScale; if(isOk) { Mat deltaRt; @@ -841,43 +859,44 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, return isOk; } +// Rotate dst by Rt (which is inverted in fact) 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, - 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) + 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 corresps(depth1.size(), CV_16SC2, Scalar::all(-1)); - Mat diffs(depth1.size(), CV_32F, Scalar::all(-1)); + Mat corresps(depthDst.size(), CV_16SC2, Scalar::all(-1)); + Mat diffs(depthDst.size(), CV_32F, Scalar::all(-1)); Matx33d K(_K), K_inv(_K_inv); - Rect r(0, 0, depth1.cols, depth1.rows); + 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(); - AutoBuffer buf(3 * (depth1.cols + depth1.rows)); + AutoBuffer buf(3 * (depthDst.cols + depthDst.rows)); float* KRK_inv0_u1 = buf.data(); - float* KRK_inv1_v1_plus_KRK_inv2 = KRK_inv0_u1 + depth1.cols; - float* KRK_inv3_u1 = KRK_inv1_v1_plus_KRK_inv2 + depth1.rows; - float* KRK_inv4_v1_plus_KRK_inv5 = KRK_inv3_u1 + depth1.cols; - float* KRK_inv6_u1 = KRK_inv4_v1_plus_KRK_inv5 + depth1.rows; - float* KRK_inv7_v1_plus_KRK_inv8 = KRK_inv6_u1 + depth1.cols; + 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; { Mat R = Rt(Rect(0, 0, 3, 3)).clone(); Mat KRK_inv = K * R * K_inv; const double* KRK_inv_ptr = KRK_inv.ptr(); - for (int u1 = 0; u1 < depth1.cols; u1++) + for (int u1 = 0; u1 < depthDst.cols; u1++) { 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); } - for (int v1 = 0; v1 < depth1.rows; v1++) + for (int v1 = 0; v1 < depthDst.rows; v1++) { 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]); @@ -887,11 +906,11 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, double sigma = 0; int correspCount = 0; - for (int v1 = 0; v1 < depth1.rows; v1++) + for (int v1 = 0; v1 < depthDst.rows; v1++) { - const float* depth1_row = depth1.ptr(v1); - const uchar* mask1_row = selectMask1.ptr(v1); - for (int u1 = 0; u1 < depth1.cols; u1++) + const float* depth1_row = depthDst.ptr(v1); + const uchar* mask1_row = selectMaskDst.ptr(v1); + for (int u1 = 0; u1 < depthDst.cols; u1++) { float d1 = depth1_row[u1]; if (mask1_row[u1] && !cvIsNaN(d1)) @@ -909,8 +928,8 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, Kt_ptr[1])); if (r.contains(Point(u0, v0))) { - float d0 = depth0.at(v0, u0); - if (validMask0.at(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff) + float d0 = depthSrc.at(v0, u0); + if (validMaskSrc.at(v0, u0) && std::abs(transformed_d1 - d0) <= maxDepthDiff) { CV_DbgAssert(!cvIsNaN(d0)); Vec2s& c = corresps.at(v0, u0); @@ -921,20 +940,20 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, diff = 0; int exist_u1 = c[0], exist_v1 = c[1]; - float exist_d1 = (float)(depth1.at(exist_v1, exist_u1) * - (KRK_inv6_u1[exist_u1] + KRK_inv7_v1_plus_KRK_inv8[exist_v1]) + Kt_ptr[2]); + 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]); if (transformed_d1 > exist_d1) continue; if (method == OdometryType::RGB) - diff = static_cast(static_cast(image0.at(v0, u0)) - - static_cast(image1.at(v1, u1))); + diff = static_cast(static_cast(imageSrc.at(v0, u0)) - + static_cast(imageDst.at(v1, u1))); } else { if (method == OdometryType::RGB) - diff = static_cast(static_cast(image0.at(v0, u0)) - - static_cast(image1.at(v1, u1))); + diff = static_cast(static_cast(imageSrc.at(v0, u0)) - + static_cast(imageDst.at(v1, u1))); correspCount++; } c = Vec2s((short)u1, (short)v1); @@ -973,16 +992,16 @@ void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, const Mat& Rt, } void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, - const Mat& dI_dx1, const Mat& dI_dy1, - const Mat& corresps, const Mat& _diffs, const double _sigma, - double fx, double fy, double sobelScaleIn, - Mat& AtA, Mat& AtB, CalcRgbdEquationCoeffsPtr func, int transformDim) + const Mat& dI_dx1, const Mat& dI_dy1, + const Mat& corresps, const Mat& _diffs, const double _sigma, + double fx, double fy, double sobelScaleIn, + Mat& AtA, Mat& AtB, OdometryTransformType transformType) { + int transformDim = getTransformDim(transformType); AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0)); AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0)); double* AtB_ptr = AtB.ptr(); - CV_Assert(Rt.type() == CV_64FC1); const double* Rt_ptr = Rt.ptr(); @@ -1010,10 +1029,11 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, 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]); - func(A_ptr, - w_sobelScale * dI_dx1.at(v1, u1), - w_sobelScale * dI_dy1.at(v1, u1), - tp0, fx, fy); + rgbdCoeffsFunc(transformType, + A_ptr, + w_sobelScale * dI_dx1.at(v1, u1), + w_sobelScale * dI_dy1.at(v1, u1), + tp0, fx, fy); for (int y = 0; y < transformDim; y++) { @@ -1031,11 +1051,13 @@ void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, AtA.at(x, y) = AtA.at(y, x); } + void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, - const Mat& cloud1, const Mat& normals1, - const Mat& corresps, - Mat& AtA, Mat& AtB, CalcICPEquationCoeffsPtr func, int transformDim) + const Mat& cloud1, const Mat& normals1, + const Mat& corresps, + Mat& AtA, Mat& AtB, double& scale, OdometryTransformType transformType) { + int transformDim = getTransformDim(transformType); AtA = Mat(transformDim, transformDim, CV_64FC1, Scalar(0)); AtB = Mat(transformDim, 1, CV_64FC1, Scalar(0)); double* AtB_ptr = AtB.ptr(); @@ -1062,9 +1084,9 @@ void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, 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]); + 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]); Vec4f n1 = normals1.at(v1, u1); Vec4f _v = cloud1.at(v1, u1); @@ -1084,18 +1106,21 @@ void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, const Vec4i& c = corresps_ptr[correspIndex]; int u1 = c[2], v1 = c[3]; - double w = sigma + std::abs(diffs_ptr[correspIndex]); + double w = sigma +std::abs(diffs_ptr[correspIndex]); w = w > DBL_EPSILON ? 1. / w : 1.; Vec4f n4 = normals1.at(v1, u1); - func(A_ptr, tps0_ptr[correspIndex], Vec3f(n4[0], n4[1], n4[2]) * w); + Vec4f p1 = cloud1.at(v1, u1); + icpCoeffsFunc(transformType, + A_ptr, tps0_ptr[correspIndex], Point3f(p1[0], p1[1], p1[2]), Vec3f(n4[0], n4[1], n4[2]) * w); for (int y = 0; y < transformDim; y++) { double* AtA_ptr = AtA.ptr(y); for (int x = y; x < transformDim; x++) + { AtA_ptr[x] += A_ptr[y] * A_ptr[x]; - + } AtB_ptr[y] += A_ptr[y] * w * diffs_ptr[correspIndex]; } } @@ -1112,7 +1137,7 @@ void computeProjectiveMatrix(const Mat& ksi, Mat& Rt) const double* ksi_ptr = ksi.ptr(); // 0.5 multiplication is here because (dual) quaternions keep half an angle/twist inside Matx44d matdq = (DualQuatd(0, ksi_ptr[0], ksi_ptr[1], ksi_ptr[2], - 0, ksi_ptr[3], ksi_ptr[4], ksi_ptr[5]) * 0.5).exp().toMat(QUAT_ASSUME_UNIT); + 0, ksi_ptr[3], ksi_ptr[4], ksi_ptr[5]) * 0.5).exp().toMat(QUAT_ASSUME_UNIT); Mat(matdq).copyTo(Rt); } diff --git a/modules/3d/src/rgbd/odometry_functions.hpp b/modules/3d/src/rgbd/odometry_functions.hpp index 67db149ea9..8b190996c2 100644 --- a/modules/3d/src/rgbd/odometry_functions.hpp +++ b/modules/3d/src/rgbd/odometry_functions.hpp @@ -12,9 +12,26 @@ namespace cv { enum class OdometryTransformType { - ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4 + // rotation, translation, rotation+translation, rotation*scale+translation + ROTATION = 1, TRANSLATION = 2, RIGID_TRANSFORMATION = 4, SIM_TRANSFORMATION = 8 }; +static inline int getTransformDim(OdometryTransformType transformType) +{ + switch(transformType) + { + case OdometryTransformType::SIM_TRANSFORMATION: + return 7; + case OdometryTransformType::RIGID_TRANSFORMATION: + return 6; + case OdometryTransformType::ROTATION: + case OdometryTransformType::TRANSLATION: + return 3; + default: + CV_Error(Error::StsBadArg, "Incorrect transformation type"); + } +} + static inline void checkImage(InputArray image) { @@ -55,18 +72,39 @@ 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) { double invz = 1. / p3d.z, - v0 = dIdx * fx * invz, - v1 = dIdy * fy * invz, - v2 = -(v0 * p3d.x + v1 * p3d.y) * invz; - - C[0] = -p3d.z * v1 + p3d.y * v2; - C[1] = p3d.z * v0 - p3d.x * v2; - C[2] = -p3d.y * v0 + p3d.x * v1; + 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; @@ -76,58 +114,125 @@ static inline void calcRgbdEquationCoeffsRotation(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; - C[0] = -p3d.z * v1 + p3d.y * v2; - C[1] = p3d.z * v0 - p3d.x * v2; - C[2] = -p3d.y * v0 + p3d.x * v1; + 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; } static inline void calcRgbdEquationCoeffsTranslation(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; + 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; } -typedef -void (*CalcRgbdEquationCoeffsPtr)(double*, double, double, const Point3f&, double, double); +static inline void rgbdCoeffsFunc(OdometryTransformType transformType, + double* C, double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) +{ + 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); + break; + case OdometryTransformType::ROTATION: + calcRgbdEquationCoeffsRotation(C, dIdx, dIdy, p3d, fx, fy); + break; + case OdometryTransformType::TRANSLATION: + calcRgbdEquationCoeffsTranslation(C, dIdx, dIdy, p3d, fx, fy); + break; + default: + CV_Error(Error::StsBadArg, "Incorrect transformation type"); + } +} + +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 Vec3f& n1) +void calcICPEquationCoeffs(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1) { - C[0] = -p0.z * n1[1] + p0.y * n1[2]; - C[1] = p0.z * n1[0] - p0.x * n1[2]; - C[2] = -p0.y * n1[0] + p0.x * n1[1]; + 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]; } static inline -void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Vec3f& n1) +void calcICPEquationCoeffsRotation(double* C, const Point3f& p0, const Point3f& /*p1*/, const Vec3f& n1) { - C[0] = -p0.z * n1[1] + p0.y * n1[2]; - C[1] = p0.z * n1[0] - p0.x * n1[2]; - C[2] = -p0.y * n1[0] + p0.x * n1[1]; + Point3d pxv = p0.cross(Point3d(n1)); + + C[0] = pxv.x; + C[1] = pxv.y; + C[2] = pxv.z; } static inline -void calcICPEquationCoeffsTranslation(double* C, const Point3f& /*p0*/, const Vec3f& n1) +void calcICPEquationCoeffsTranslation(double* C, const Point3f& /*p0*/, const Point3f& /*p1*/, const Vec3f& n1) { C[0] = n1[0]; C[1] = n1[1]; C[2] = n1[2]; } -typedef -void (*CalcICPEquationCoeffsPtr)(double*, const Point3f&, const Vec3f&); +static inline +void icpCoeffsFunc(OdometryTransformType transformType, double* C, const Point3f& p0, const Point3f& p1, const Vec3f& n1) +{ + switch(transformType) + { + case OdometryTransformType::SIM_TRANSFORMATION: + calcICPScaleEquationCoeffs(C, p0, p1, n1); + break; + case OdometryTransformType::RIGID_TRANSFORMATION: + calcICPEquationCoeffs(C, p0, p1, n1);; + break; + case OdometryTransformType::ROTATION: + calcICPEquationCoeffsRotation(C, p0, p1, n1);; + break; + case OdometryTransformType::TRANSLATION: + calcICPEquationCoeffsTranslation(C, p0, p1, n1);; + break; + default: + CV_Error(Error::StsBadArg, "Incorrect transformation type"); + } +} void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype); void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, bool useDepth); @@ -159,47 +264,48 @@ template void preparePyramidSobel(InputArrayOfArrays pyramidImage, int dx, int dy, InputOutputArrayOfArrays pyramidSobel, int sobelSize); void preparePyramidTexturedMask(InputArrayOfArrays pyramid_dI_dx, InputArrayOfArrays pyramid_dI_dy, - InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale); + InputArray minGradMagnitudes, InputArrayOfArrays pyramidMask, double maxPointsPart, + InputOutputArrayOfArrays pyramidTexturedMask, double sobelScale); void randomSubsetOfMask(InputOutputArray _mask, float part); void preparePyramidNormals(InputArray normals, InputArrayOfArrays pyramidDepth, InputOutputArrayOfArrays pyramidNormals); void preparePyramidNormalsMask(InputArray pyramidNormals, InputArray pyramidMask, double maxPointsPart, - InputOutputArrayOfArrays /*std::vector&*/ pyramidNormalsMask); + InputOutputArrayOfArrays /*std::vector&*/ pyramidNormalsMask); -bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, - const OdometryFrame srcFrame, - const OdometryFrame dstFrame, - const Matx33f& cameraMatrix, - float maxDepthDiff, float angleThreshold, const std::vector& iterCounts, - double maxTranslation, double maxRotation, double sobelScale, - OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype); +// scale = 0, if not needs scale; otherwise scale = 1; +bool RGBDICPOdometryImpl(OutputArray _Rt, float& scale, const Mat& initRt, + const OdometryFrame srcFrame, + const OdometryFrame dstFrame, + const Matx33f& cameraMatrix, + float maxDepthDiff, float angleThreshold, const std::vector& iterCounts, + double maxTranslation, double maxRotation, double sobelScale, + OdometryType method, OdometryTransformType transfromType, OdometryAlgoType algtype); void computeCorresps(const Matx33f& _K, const Matx33f& _K_inv, 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); + 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); void calcRgbdLsmMatrices(const Mat& cloud0, const Mat& Rt, - const Mat& dI_dx1, const Mat& dI_dy1, - const Mat& corresps, const Mat& diffs, const double sigma, - double fx, double fy, double sobelScaleIn, - Mat& AtA, Mat& AtB, CalcRgbdEquationCoeffsPtr func, int transformDim); + const Mat& dI_dx1, const Mat& dI_dy1, + const Mat& corresps, const Mat& diffs, const double sigma, + double fx, double fy, double sobelScaleIn, + Mat& AtA, Mat& AtB, OdometryTransformType transformType); void calcICPLsmMatrices(const Mat& cloud0, const Mat& Rt, - const Mat& cloud1, const Mat& normals1, - const Mat& corresps, - Mat& AtA, Mat& AtB, CalcICPEquationCoeffsPtr func, int transformDim); + const Mat& cloud1, const Mat& normals1, + const Mat& corresps, + Mat& AtA, Mat& AtB, double& scale, 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); + cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b); #ifdef HAVE_OPENCL bool ocl_calcICPLsmMatricesFast(Matx33f cameraMatrix, const UMat& oldPts, const UMat& oldNrm, const UMat& newPts, const UMat& newNrm, - cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b); + cv::Affine3f pose, int level, float maxDepthDiff, float angleThreshold, cv::Matx66f& A, cv::Vec6f& b); #endif void computeProjectiveMatrix(const Mat& ksi, Mat& Rt); diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 00da12a544..d58b1d596b 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -124,11 +124,13 @@ public: OdometryAlgoType _algtype, double _maxError1, double _maxError5, + bool _testScale = false, double _idError = DBL_EPSILON) : otype(_otype), algtype(_algtype), maxError1(_maxError1), maxError5(_maxError5), + testScale(_testScale), idError(_idError) { } @@ -154,6 +156,7 @@ public: OdometryAlgoType algtype; double maxError1; double maxError5; + bool testScale; double idError; }; @@ -248,21 +251,27 @@ 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 = odometry.compute(odf, odf, calcRt); + bool isComputed; + if (testScale) + isComputed = odometry.compute(odf, odf, calcRt, scale); + else + isComputed = odometry.compute(odf, odf, calcRt); if(!isComputed) { FAIL() << "Can not find Rt between the same frame" << std::endl; } double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1)); - if(ndiff > idError) + float sdiff = abs(scale - 1.f); + if (ndiff > idError && abs(scale - 1.f) < FLT_EPSILON) { - FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << std::endl; + FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << " sdiff = " << sdiff << std::endl; } // 2. Generate random rigid body motion in some ranges several times (iterCount). @@ -279,23 +288,36 @@ void OdometryTest::run() Mat rvec, tvec; generateRandomTransformation(rvec, tvec); - Mat warpedImage, warpedDepth; - warpFrame(image, depth, rvec, tvec, K, warpedImage, warpedDepth); + 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 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); - isComputed = odometry.compute(odfSrc, odfDst, calcRt); + if (testScale) + isComputed = odometry.compute(odfSrc, odfDst, calcRt, scale); + else + isComputed = odometry.compute(odfSrc, odfDst, calcRt); if (!isComputed) + { + CV_LOG_INFO(NULL, "Iter " << iter << "; Odometry compute returned false"); continue; - Mat calcR = calcRt(Rect(0,0,3,3)), calcRvec; + } + Mat calcR = calcRt(Rect(0, 0, 3, 3)), calcRvec; cv::Rodrigues(calcR, calcRvec); calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows); Mat calcTvec = calcRt(Rect(3,0,1,3)); @@ -312,6 +334,8 @@ 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)); @@ -320,16 +344,15 @@ void OdometryTest::run() double rdiffnorm = cv::norm(diff.rvec()); double tdiffnorm = cv::norm(diff.translation()); - if (rdiffnorm < possibleError && tdiffnorm < possibleError) - { + if (rdiffnorm < possibleError && tdiffnorm < possibleError && abs(scale - test_scale) < scale_error) better_1time_count++; - } - if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError) + if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError && abs(scale - test_scale) < scale_error) 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); } @@ -400,6 +423,12 @@ 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); @@ -408,7 +437,7 @@ TEST(RGBD_Odometry_RgbdICP, algorithmic) TEST(RGBD_Odometry_FastICP, algorithmic) { - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON); + OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON); test.run(); } @@ -433,7 +462,7 @@ TEST(RGBD_Odometry_RgbdICP, UMats) TEST(RGBD_Odometry_FastICP, UMats) { - OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON); + OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, false, FLT_EPSILON); test.checkUMats(); } diff --git a/samples/python/odometry.py b/samples/python/odometry.py index 648a018763..cf9808c379 100644 --- a/samples/python/odometry.py +++ b/samples/python/odometry.py @@ -13,6 +13,7 @@ def main(): help="""DEPTH - works with depth, RGB - works with images, RGB_DEPTH - works with all, + SCALE - works with depth and calculate Rt with scale, default - runs all algos""", default="") parser.add_argument( @@ -34,7 +35,7 @@ def main(): args = parser.parse_args() - if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" or args.algo == "": + if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" or args.algo == "SCALE" or args.algo == "": source_depth_frame = cv.samples.findFile(args.source_depth_frame) destination_depth_frame = cv.samples.findFile(args.destination_depth_frame) depth1 = cv.imread(source_depth_frame, cv.IMREAD_ANYDEPTH).astype(np.float32) @@ -50,18 +51,24 @@ def main(): odometry = cv.Odometry(cv.DEPTH) Rt = np.zeros((4, 4)) odometry.compute(depth1, depth2, Rt) - print(Rt) + print("Rt:\n {}".format(Rt)) if args.algo == "RGB" or args.algo == "": odometry = cv.Odometry(cv.RGB) Rt = np.zeros((4, 4)) odometry.compute(rgb1, rgb2, Rt) - print(Rt) + print("Rt:\n {}".format(Rt)) if args.algo == "RGB_DEPTH" or args.algo == "": odometry = cv.Odometry(cv.RGB_DEPTH) Rt = np.zeros((4, 4)) odometry.compute(depth1, rgb1, depth2, rgb2, Rt) - print(Rt) - + print("Rt:\n {}".format(Rt)) + if args.algo == "SCALE" or args.algo == "": + print(args.algo) + odometry = cv.Odometry() + Rt = np.zeros((4, 4)) + scale = np.zeros((1, 1)) + odometry.compute(depth1, depth2*1.05, Rt, scale) + print("Rt:\n {}\nScale: {}".format(Rt, scale)) if __name__ == '__main__': From bee410c748d432f49c59dfee2d1a8a92ae91604b Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 23 Jun 2022 23:18:59 +0200 Subject: [PATCH 02/22] warpFrame() test draft + script generating test data --- modules/3d/misc/python/warp_test.py | 113 ++++++++++++++++++++++++++++ modules/3d/src/rgbd/odometry.cpp | 26 +++++-- modules/3d/test/test_odometry.cpp | 57 ++++++++++++++ 3 files changed, 188 insertions(+), 8 deletions(-) create mode 100644 modules/3d/misc/python/warp_test.py diff --git a/modules/3d/misc/python/warp_test.py b/modules/3d/misc/python/warp_test.py new file mode 100644 index 0000000000..40fc844506 --- /dev/null +++ b/modules/3d/misc/python/warp_test.py @@ -0,0 +1,113 @@ +import numpy as np +from scipy.spatial.transform import Rotation +import imageio +# optional, works slower w/o it +from numba import jit + +depthFactor = 5000 +psize = (640, 480) +fx = 525.0 +fy = 525.0 +cx = psize[0]/2-0.5 +cy = psize[1]/2-0.5 +K = np.array([[fx, 0, cx], + [ 0, fy, cy], + [ 0, 0, 1]]) +# some random transform +rmat = Rotation.from_rotvec(np.array([0.1, 0.2, 0.3])).as_dcm() +tmat = np.array([[-0.04, 0.05, 0.6]]).T +rtmat = np.vstack((np.hstack((rmat, tmat)), np.array([[0, 0, 0, 1]]))) + +#TODO: warp rgb image as well +testDataPath = "/path/to/sources/opencv_extra/testdata" +srcDepth = imageio.imread(testDataPath + "/cv/rgbd/depth.png") + +@jit +def reproject(image, df, K): + Kinv = np.linalg.inv(K) + xsz, ysz = image.shape[1], image.shape[0] + reprojected = np.zeros((ysz, xsz, 3)) + for y in range(ysz): + for x in range(xsz): + z = image[y, x]/df + + v = Kinv @ np.array([x*z, y*z, z]).T + + #xv = (x - cx)/fx * z + #yv = (y - cy)/fy * z + #zv = z + + reprojected[y, x, :] = v[:] + return reprojected + +@jit +def reprojectRtProject(image, K, depthFactor, rmat, tmat): + Kinv = np.linalg.inv(K) + xsz, ysz = image.shape[1], image.shape[0] + projected = np.zeros((ysz, xsz, 3)) + for y in range(ysz): + for x in range(xsz): + z = image[y, x]/depthFactor + + v = K @ (rmat @ Kinv @ np.array([x*z, y*z, z]).T + tmat[:, 0]) + + projected[y, x, :] = v[:] + + return projected + +@jit +def reprojectRt(image, K, depthFactor, rmat, tmat): + Kinv = np.linalg.inv(K) + xsz, ysz = image.shape[1], image.shape[0] + rotated = np.zeros((ysz, xsz, 3)) + for y in range(ysz): + for x in range(xsz): + z = image[y, x]/depthFactor + + v = rmat @ Kinv @ np.array([x*z, y*z, z]).T + tmat[:, 0] + + rotated[y, x, :] = v[:] + + return rotated + +# put projected points on a depth map +@jit +def splat(projected, maxv): + xsz, ysz = projected.shape[1], projected.shape[0] + depth = np.full((ysz, xsz), maxv, np.float32) + for y in range(ysz): + for x in range(xsz): + p = projected[y, x, :] + z = p[2] + if z > 0: + u, v = int(p[0]/z), int(p[1]/z) + okuv = (u >= 0 and v >= 0 and u < xsz and v < ysz) + if okuv and depth[v, u] > z: + depth[v, u] = z + return depth + +maxv = depthFactor +im2 = splat(reprojectRtProject(srcDepth, K, depthFactor, rmat, tmat), maxv) +im2[im2 >= maxv] = 0 +im2 = im2*depthFactor + +imageio.imwrite(testDataPath + "/cv/rgbd/warped.png", im2) + +# debug + +outObj = False +def outFile(path, ptsimg): + f = open(path, "w") + for y in range(ptsimg.shape[0]): + for x in range(ptsimg.shape[1]): + v = ptsimg[y, x, :] + if v[2] > 0: + f.write(f"v {v[0]} {v[1]} {v[2]}\n") + f.close() + +if outObj: + objdir = "/path/to/objdir/" + outFile(objdir + "reproj_rot_proj.obj", reproject(im2, depthFactor, K)) + outFile(objdir + "rotated.obj", reprojectRt(srcDepth, K, depthFactor, rmat, tmat)) + + diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index 7afe1eca5a..ac0b184c92 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -381,14 +381,18 @@ bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, template static void warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, - const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff, - OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask) + const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff, + OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask) { + //TODO: take into account that image can be empty + //TODO: mask can also be empty + CV_Assert(_image.size() == depth.size()); Mat cloud; depthTo3d(depth, cameraMatrix, cloud); + //TODO: replace this by channel split/merge after the function is covered by tests Mat cloud3; cloud3.create(cloud.size(), CV_32FC3); for (int y = 0; y < cloud3.rows; y++) @@ -400,11 +404,13 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, } } + //TODO: do the following instead of the functions: K*R*Kinv*[uv1]*z + K*t + //TODO: optimize it using K's structure std::vector points2d; Mat transformedCloud; perspectiveTransform(cloud3, transformedCloud, Rt); projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix, - distCoeff, points2d); + distCoeff, points2d); Mat image = _image.getMat(); Size sz = _image.size(); @@ -426,7 +432,8 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, { const float transformed_z = transformedCloud_row[x].z; const Point2i p2d = points2d_row[x]; - if ((!mask_row || mask_row[x]) && transformed_z > 0 && rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at(p2d) > transformed_z) + if ((!mask_row || mask_row[x]) && transformed_z > 0 && + rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at(p2d) > transformed_z) { warpedImage.at(p2d) = image_row[x]; zBuffer.at(p2d) = transformed_z; @@ -444,14 +451,17 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, } } + void warpFrame(InputArray image, InputArray depth, InputArray mask, - InputArray Rt, InputArray cameraMatrix, InputArray distCoeff, - OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask) + InputArray Rt, InputArray cameraMatrix, InputArray distCoeff, + OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask) { if (image.type() == CV_8UC1) - warpFrameImpl(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), warpedImage, warpedDepth, warpedMask); + warpFrameImpl(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), + warpedImage, warpedDepth, warpedMask); else if (image.type() == CV_8UC3) - warpFrameImpl >(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), warpedImage, warpedDepth, warpedMask); + warpFrameImpl>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), + warpedImage, warpedDepth, warpedMask); else CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3"); } diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index d58b1d596b..d8f81fa13d 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -491,4 +491,61 @@ TEST(RGBD_Odometry_FastICP, prepareFrame) test.prepareFrameCheck(); } +TEST(RGBD_Odometry_WarpFrame, compareToGold) +{ + //TODO: identity transform + //TODO: finish it + + std::string dataPath = cvtest::TS::ptr()->get_data_path(); + std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png"; + // The depth was generated using the script at 3d/misc/python/warp_test.py + std::string warpedDepthFilename = dataPath + "/cv/rgbd/warped.png"; + + Mat srcDepth, warpedDepth; + + srcDepth = imread(srcDepthFilename, -1); + if(srcDepth.empty()) + { + FAIL() << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl; + } + + warpedDepth = imread(warpedDepthFilename, -1); + if(warpedDepth.empty()) + { + FAIL() << "Depth " << warpedDepthFilename.c_str() << "can not be read" << std::endl; + } + + CV_DbgAssert(srcDepth.type() == CV_16UC1); + CV_DbgAssert(warpedDepth.type() == CV_16UC1); + + double fx = 525.0, fy = 525.0, + cx = 319.5, cy = 239.5; + Matx33d K(fx, 0, cx, + 0, fy, cy, + 0, 0, 1); + cv::Affine3d rt(cv::Vec3d(0.1, 0.2, 0.3), cv::Vec3d(-0.04, 0.05, 0.6)); + + //TODO: check with and without scaling + Mat srcDepthCvt, warpedDepthCvt; + srcDepth.convertTo(srcDepthCvt, CV_32FC1, 1.f/5000.f); + srcDepth = srcDepthCvt; + warpedDepth.convertTo(warpedDepthCvt, CV_32FC1, 1.f/5000.f); + warpedDepth = warpedDepthCvt; + + srcDepth.setTo(std::numeric_limits::quiet_NaN(), srcDepth < FLT_EPSILON); + warpedDepth.setTo(std::numeric_limits::quiet_NaN(), warpedDepth < FLT_EPSILON); + + //TODO: check with and without image + //TODO: check with and without mask + //TODO: check with and without distCoeff + Mat image, mask, distCoeff, dstImage, dstDepth, dstMask; + warpFrame(image, srcDepth, mask, rt.matrix, K, distCoeff, + dstImage, dstDepth, dstMask); + + //TODO: check this norm + double depthDiff = cv::norm(dstDepth, warpedDepth, NORM_L2); + //TODO: find true threshold, maybe based on pixcount + ASSERT_LE(0.1, depthDiff); +} + }} // namespace From c12d4c82dfe59e25694c3e71e884600efe573f5f Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 23 Jun 2022 23:25:30 +0200 Subject: [PATCH 03/22] python Odometry scale test removed --- modules/3d/misc/python/test/test_odometry.py | 31 -------------------- 1 file changed, 31 deletions(-) diff --git a/modules/3d/misc/python/test/test_odometry.py b/modules/3d/misc/python/test/test_odometry.py index b961f361fd..cc16725d20 100644 --- a/modules/3d/misc/python/test/test_odometry.py +++ b/modules/3d/misc/python/test/test_odometry.py @@ -108,36 +108,5 @@ class odometry_test(NewOpenCVTests): self.assertLessEqual(res, eps) self.assertTrue(isCorrect) - def test_OdometryScale(self): - depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) - radian = np.radians(1) - Rt_warp = np.array( - [[np.cos(radian), -np.sin(radian), 0], - [np.sin(radian), np.cos(radian), 0], - [0, 0, 1]], dtype=np.float32 - ) - Rt_curr = np.array( - [[np.cos(radian), -np.sin(radian), 0, 0], - [np.sin(radian), np.cos(radian), 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1]], dtype=np.float32 - ) - Rt_res = np.zeros((4, 4)) - scale = 1.01 - scale_res = np.zeros((1, 1)) - - odometry = cv.Odometry() - warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) - - isCorrect = odometry.compute(depth, warped_depth*scale, Rt_res, scale_res) - Rt_diff = np.absolute(Rt_curr - Rt_res).sum() - scale_diff = np.absolute(scale - scale_res[0][0]) - - Rt_eps = 0.2 - scale_eps = 0.1 - self.assertLessEqual(Rt_diff, Rt_eps) - self.assertLessEqual(scale_diff, scale_eps) - self.assertTrue(isCorrect) - if __name__ == '__main__': NewOpenCVTests.bootstrap() From 770c0d14162ab069ca125bfc7fb596286701c610 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 23 Jun 2022 23:47:08 +0200 Subject: [PATCH 04/22] 1. removed (almost all) additional scale parameter mentions 2. refactored funcptrs to switch/cases & more --- modules/3d/include/opencv2/3d/odometry.hpp | 14 -- modules/3d/src/rgbd/odometry.cpp | 62 +++--- modules/3d/src/rgbd/odometry_functions.cpp | 216 +++++++++------------ modules/3d/src/rgbd/odometry_functions.hpp | 146 +++++--------- modules/3d/test/test_odometry.cpp | 33 +--- 5 files changed, 174 insertions(+), 297 deletions(-) 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(); } From 4e1ce3e0ebbfd40f06863bb66c795b3376935bba Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 23 Jun 2022 23:51:16 +0200 Subject: [PATCH 05/22] odometry python sample got rid of scale parameter --- samples/python/odometry.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/samples/python/odometry.py b/samples/python/odometry.py index cf9808c379..84ec31fea2 100644 --- a/samples/python/odometry.py +++ b/samples/python/odometry.py @@ -35,7 +35,7 @@ def main(): args = parser.parse_args() - if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" or args.algo == "SCALE" or args.algo == "": + if args.algo == "RGB_DEPTH" or args.algo == "DEPTH" or args.algo == "": source_depth_frame = cv.samples.findFile(args.source_depth_frame) destination_depth_frame = cv.samples.findFile(args.destination_depth_frame) depth1 = cv.imread(source_depth_frame, cv.IMREAD_ANYDEPTH).astype(np.float32) @@ -62,13 +62,6 @@ def main(): Rt = np.zeros((4, 4)) odometry.compute(depth1, rgb1, depth2, rgb2, Rt) print("Rt:\n {}".format(Rt)) - if args.algo == "SCALE" or args.algo == "": - print(args.algo) - odometry = cv.Odometry() - Rt = np.zeros((4, 4)) - scale = np.zeros((1, 1)) - odometry.compute(depth1, depth2*1.05, Rt, scale) - print("Rt:\n {}\nScale: {}".format(Rt, scale)) if __name__ == '__main__': From bf8f7b4e57268ab21be6530779fe2e84e5398a1a Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 23 Jun 2022 23:53:57 +0200 Subject: [PATCH 06/22] more scale stuff removed --- modules/3d/test/test_odometry.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 498a75d421..155b463a11 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -124,13 +124,11 @@ public: OdometryAlgoType _algtype, double _maxError1, double _maxError5, - bool _testScale = false, double _idError = DBL_EPSILON) : otype(_otype), algtype(_algtype), maxError1(_maxError1), maxError5(_maxError5), - testScale(_testScale), idError(_idError) { } @@ -175,7 +173,7 @@ void OdometryTest::readData(Mat& image, Mat& depth) const } if(depth.empty()) { - FAIL() << "Depth" << depthFilename.c_str() << "can not be read" << std::endl; + FAIL() << "Depth " << depthFilename.c_str() << "can not be read" << std::endl; } CV_DbgAssert(image.type() == CV_8UC1); @@ -263,10 +261,9 @@ void OdometryTest::run() FAIL() << "Can not find Rt between the same frame" << std::endl; } double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1)); - float sdiff = abs(scale - 1.f); - if (ndiff > idError && abs(scale - 1.f) < FLT_EPSILON) + if (ndiff > idError) { - FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << " sdiff = " << sdiff << std::endl; + FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << std::endl; } // 2. Generate random rigid body motion in some ranges several times (iterCount). From d56e8f053f14b979bda20245b494aae815dd79af Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 26 Jun 2022 22:33:31 +0200 Subject: [PATCH 07/22] python depth generator: no numba, imageio -> PIL, requirements.txt --- modules/3d/misc/python/requirements.txt | 3 +++ modules/3d/misc/python/warp_test.py | 35 +++++++++++-------------- 2 files changed, 19 insertions(+), 19 deletions(-) create mode 100644 modules/3d/misc/python/requirements.txt diff --git a/modules/3d/misc/python/requirements.txt b/modules/3d/misc/python/requirements.txt new file mode 100644 index 0000000000..a4567695c0 --- /dev/null +++ b/modules/3d/misc/python/requirements.txt @@ -0,0 +1,3 @@ +numpy>=1.17.4 +scipy>=1.3.3 +PIL>=7.0.0 diff --git a/modules/3d/misc/python/warp_test.py b/modules/3d/misc/python/warp_test.py index 40fc844506..633fa55136 100644 --- a/modules/3d/misc/python/warp_test.py +++ b/modules/3d/misc/python/warp_test.py @@ -1,8 +1,7 @@ import numpy as np from scipy.spatial.transform import Rotation -import imageio -# optional, works slower w/o it -from numba import jit +from PIL import Image +import os depthFactor = 5000 psize = (640, 480) @@ -13,16 +12,16 @@ cy = psize[1]/2-0.5 K = np.array([[fx, 0, cx], [ 0, fy, cy], [ 0, 0, 1]]) + # some random transform rmat = Rotation.from_rotvec(np.array([0.1, 0.2, 0.3])).as_dcm() tmat = np.array([[-0.04, 0.05, 0.6]]).T rtmat = np.vstack((np.hstack((rmat, tmat)), np.array([[0, 0, 0, 1]]))) -#TODO: warp rgb image as well -testDataPath = "/path/to/sources/opencv_extra/testdata" -srcDepth = imageio.imread(testDataPath + "/cv/rgbd/depth.png") +testDataPath = os.getenv("OPENCV_TEST_DATA_PATH", default=None) +srcDepth = np.asarray(Image.open(testDataPath + "/cv/rgbd/depth.png")) +srcRgb = np.asarray(Image.open(testDataPath + "/cv/rgbd/rgb.png")) -@jit def reproject(image, df, K): Kinv = np.linalg.inv(K) xsz, ysz = image.shape[1], image.shape[0] @@ -40,7 +39,6 @@ def reproject(image, df, K): reprojected[y, x, :] = v[:] return reprojected -@jit def reprojectRtProject(image, K, depthFactor, rmat, tmat): Kinv = np.linalg.inv(K) xsz, ysz = image.shape[1], image.shape[0] @@ -55,7 +53,6 @@ def reprojectRtProject(image, K, depthFactor, rmat, tmat): return projected -@jit def reprojectRt(image, K, depthFactor, rmat, tmat): Kinv = np.linalg.inv(K) xsz, ysz = image.shape[1], image.shape[0] @@ -71,10 +68,10 @@ def reprojectRt(image, K, depthFactor, rmat, tmat): return rotated # put projected points on a depth map -@jit -def splat(projected, maxv): +def splat(projected, maxv, rgb): xsz, ysz = projected.shape[1], projected.shape[0] depth = np.full((ysz, xsz), maxv, np.float32) + colors = np.full((ysz, xsz, 3), 0, np.uint8) for y in range(ysz): for x in range(xsz): p = projected[y, x, :] @@ -84,18 +81,18 @@ def splat(projected, maxv): okuv = (u >= 0 and v >= 0 and u < xsz and v < ysz) if okuv and depth[v, u] > z: depth[v, u] = z - return depth + colors[v, u, :] = rgb[y, x, :] + return depth, colors maxv = depthFactor -im2 = splat(reprojectRtProject(srcDepth, K, depthFactor, rmat, tmat), maxv) -im2[im2 >= maxv] = 0 -im2 = im2*depthFactor +dstDepth, dstRgb = splat(reprojectRtProject(srcDepth, K, depthFactor, rmat, tmat), maxv, srcRgb) +dstDepth[dstDepth >= maxv] = 0 +dstDepth = (dstDepth*depthFactor).astype(np.uint16) -imageio.imwrite(testDataPath + "/cv/rgbd/warped.png", im2) +Image.fromarray(dstDepth).save(testDataPath + "/cv/rgbd/warpedDepth.png") +Image.fromarray(dstRgb ).save(testDataPath + "/cv/rgbd/warpedRgb.png") # debug - -outObj = False def outFile(path, ptsimg): f = open(path, "w") for y in range(ptsimg.shape[0]): @@ -105,9 +102,9 @@ def outFile(path, ptsimg): f.write(f"v {v[0]} {v[1]} {v[2]}\n") f.close() +outObj = False if outObj: objdir = "/path/to/objdir/" outFile(objdir + "reproj_rot_proj.obj", reproject(im2, depthFactor, K)) outFile(objdir + "rotated.obj", reprojectRt(srcDepth, K, depthFactor, rmat, tmat)) - From 162bd5be4c1aa3c2ed1d2ea40c8091eb018c8b1f Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 27 Jun 2022 01:23:07 +0200 Subject: [PATCH 08/22] distCoeffs removed from warpFrame() signature --- modules/3d/include/opencv2/3d/depth.hpp | 3 +-- modules/3d/src/rgbd/odometry.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/modules/3d/include/opencv2/3d/depth.hpp b/modules/3d/include/opencv2/3d/depth.hpp index 735bfe4a7a..c0150384d4 100644 --- a/modules/3d/include/opencv2/3d/depth.hpp +++ b/modules/3d/include/opencv2/3d/depth.hpp @@ -132,12 +132,11 @@ CV_EXPORTS_W void rescaleDepth(InputArray in, int type, OutputArray out, double * @param mask The mask of used pixels (of CV_8UC1), it can be empty * @param Rt The transformation that will be applied to the 3d points computed from the depth * @param cameraMatrix Camera matrix - * @param distCoeff Distortion coefficients * @param warpedImage The warped image. * @param warpedDepth The warped depth. * @param warpedMask The warped mask. */ -CV_EXPORTS_W void warpFrame(InputArray image, InputArray depth, InputArray mask, InputArray Rt, InputArray cameraMatrix, InputArray distCoeff, +CV_EXPORTS_W void warpFrame(InputArray image, InputArray depth, InputArray mask, InputArray Rt, InputArray cameraMatrix, OutputArray warpedImage, OutputArray warpedDepth = noArray(), OutputArray warpedMask = noArray()); enum RgbdPlaneMethod diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index 6fc24d5175..a8ae22721b 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -365,7 +365,7 @@ bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, template static void warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, - const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff, + const Mat& Rt, const Mat& cameraMatrix, OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask) { //TODO: take into account that image can be empty @@ -394,7 +394,7 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, Mat transformedCloud; perspectiveTransform(cloud3, transformedCloud, Rt); projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix, - distCoeff, points2d); + Mat::zeros(1, 5, CV_64FC1), points2d); Mat image = _image.getMat(); Size sz = _image.size(); @@ -437,14 +437,14 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, void warpFrame(InputArray image, InputArray depth, InputArray mask, - InputArray Rt, InputArray cameraMatrix, InputArray distCoeff, + InputArray Rt, InputArray cameraMatrix, OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask) { if (image.type() == CV_8UC1) - warpFrameImpl(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), + warpFrameImpl(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), warpedImage, warpedDepth, warpedMask); else if (image.type() == CV_8UC3) - warpFrameImpl>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), + warpFrameImpl>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), warpedImage, warpedDepth, warpedMask); else CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3"); From 6b2d1033bd0da92f462623f9368e7d4186f96fa3 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Mon, 27 Jun 2022 01:24:39 +0200 Subject: [PATCH 09/22] warpFrame() test: more test cases --- modules/3d/test/test_odometry.cpp | 110 ++++++++++++++++++++++-------- 1 file changed, 83 insertions(+), 27 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 155b463a11..d0f803ac4e 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -467,61 +467,117 @@ TEST(RGBD_Odometry_FastICP, prepareFrame) test.prepareFrameCheck(); } -TEST(RGBD_Odometry_WarpFrame, compareToGold) -{ - //TODO: identity transform - //TODO: finish it +void warpFrameTest(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform) +{ std::string dataPath = cvtest::TS::ptr()->get_data_path(); std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png"; + std::string srcRgbFilename = dataPath + "/cv/rgbd/rgb.png"; // The depth was generated using the script at 3d/misc/python/warp_test.py - std::string warpedDepthFilename = dataPath + "/cv/rgbd/warped.png"; + std::string warpedDepthFilename = dataPath + "/cv/rgbd/warpedDepth.png"; + std::string warpedRgbFilename = dataPath + "/cv/rgbd/warpedRgb.png"; - Mat srcDepth, warpedDepth; + Mat srcDepth, srcRgb, warpedDepth, warpedRgb; - srcDepth = imread(srcDepthFilename, -1); - if(srcDepth.empty()) + srcDepth = imread(srcDepthFilename, IMREAD_UNCHANGED); + ASSERT_FALSE(srcDepth.empty()) << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl; + + if (identityTransform) { - FAIL() << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl; + warpedDepth = srcDepth; } - - warpedDepth = imread(warpedDepthFilename, -1); - if(warpedDepth.empty()) + else { - FAIL() << "Depth " << warpedDepthFilename.c_str() << "can not be read" << std::endl; + warpedDepth = imread(warpedDepthFilename, IMREAD_UNCHANGED); + ASSERT_FALSE(warpedDepth.empty()) << "Depth " << warpedDepthFilename.c_str() << "can not be read" << std::endl; } - CV_DbgAssert(srcDepth.type() == CV_16UC1); - CV_DbgAssert(warpedDepth.type() == CV_16UC1); + ASSERT_TRUE(srcDepth.type() == CV_16UC1); + ASSERT_TRUE(warpedDepth.type() == CV_16UC1); + + if (needRgb) + { + srcRgb = imread(srcRgbFilename); + ASSERT_FALSE(srcRgb.empty()) << "Image " << srcRgbFilename.c_str() << "can not be read" << std::endl; + + if (identityTransform) + { + warpedRgb = srcRgb; + } + else + { + warpedRgb = imread(warpedRgbFilename); + ASSERT_FALSE (warpedRgb.empty()) << "Image " << warpedRgbFilename.c_str() << "can not be read" << std::endl; + } + + ASSERT_TRUE(warpedRgb.type() == CV_8UC3); + } + else + { + srcRgb = Mat(); warpedRgb = Mat(); + } double fx = 525.0, fy = 525.0, cx = 319.5, cy = 239.5; Matx33d K(fx, 0, cx, 0, fy, cy, 0, 0, 1); - cv::Affine3d rt(cv::Vec3d(0.1, 0.2, 0.3), cv::Vec3d(-0.04, 0.05, 0.6)); + cv::Affine3d rt; + rt = identityTransform ? cv::Affine3d() : cv::Affine3d(cv::Vec3d(0.1, 0.2, 0.3), cv::Vec3d(-0.04, 0.05, 0.6)); - //TODO: check with and without scaling + float scaleCoeff = scaleDown ? 1.f/5000.f : 1.f; Mat srcDepthCvt, warpedDepthCvt; - srcDepth.convertTo(srcDepthCvt, CV_32FC1, 1.f/5000.f); + srcDepth.convertTo(srcDepthCvt, CV_32FC1, scaleCoeff); srcDepth = srcDepthCvt; - warpedDepth.convertTo(warpedDepthCvt, CV_32FC1, 1.f/5000.f); + warpedDepth.convertTo(warpedDepthCvt, CV_32FC1, scaleCoeff); warpedDepth = warpedDepthCvt; - srcDepth.setTo(std::numeric_limits::quiet_NaN(), srcDepth < FLT_EPSILON); - warpedDepth.setTo(std::numeric_limits::quiet_NaN(), warpedDepth < FLT_EPSILON); + //TODO: check that NaNs in depth work the same as explicit mask + Mat epsSrc = srcDepth >= FLT_EPSILON, epsWarped = warpedDepth >= FLT_EPSILON; + Mat srcMask, warpedMask; + if (checkMask) + { + srcMask = epsSrc; warpedMask = epsWarped; + } + else + { + srcMask = Mat(); warpedMask = Mat(); + } + srcDepth.setTo(std::numeric_limits::quiet_NaN(), ~epsSrc); + warpedDepth.setTo(std::numeric_limits::quiet_NaN(), ~epsWarped); + + Mat distCoeff, dstRgb, dstDepth, dstMask; + warpFrame(srcRgb, srcDepth, srcMask, rt.matrix, K, + dstRgb, dstDepth, dstMask); - //TODO: check with and without image - //TODO: check with and without mask - //TODO: check with and without distCoeff - Mat image, mask, distCoeff, dstImage, dstDepth, dstMask; - warpFrame(image, srcDepth, mask, rt.matrix, K, distCoeff, - dstImage, dstDepth, dstMask); + //TODO: finish it //TODO: check this norm double depthDiff = cv::norm(dstDepth, warpedDepth, NORM_L2); + double rgbDiff = cv::norm(dstRgb, warpedRgb, NORM_L2); + double maskDiff = cv::norm(dstMask, warpedMask, NORM_L2); //TODO: find true threshold, maybe based on pixcount ASSERT_LE(0.1, depthDiff); } +TEST(RGBD_Odometry_WarpFrame, identity) +{ + warpFrameTest(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ true); +} + +TEST(RGBD_Odometry_WarpFrame, noRgb) +{ + warpFrameTest(/* needRgb */ false, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ false); +} + +TEST(RGBD_Odometry_WarpFrame, nansAreMasked) +{ + warpFrameTest(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ false, /* identityTransform */ false); +} + +TEST(RGBD_Odometry_WarpFrame, bigScale) +{ + warpFrameTest(/* needRgb */ true, /* scaleDown*/ false, /* checkMask */ true, /* identityTransform */ false); +} + }} // namespace From 44c0b582587d7c2a7e7da4db5788f84e1bf468f8 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 29 Jun 2022 23:45:33 +0200 Subject: [PATCH 10/22] depth generator fixed at absent values --- modules/3d/misc/python/warp_test.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/modules/3d/misc/python/warp_test.py b/modules/3d/misc/python/warp_test.py index 633fa55136..b8dda4fd5f 100644 --- a/modules/3d/misc/python/warp_test.py +++ b/modules/3d/misc/python/warp_test.py @@ -49,7 +49,8 @@ def reprojectRtProject(image, K, depthFactor, rmat, tmat): v = K @ (rmat @ Kinv @ np.array([x*z, y*z, z]).T + tmat[:, 0]) - projected[y, x, :] = v[:] + if z > 0: + projected[y, x, :] = v[:] return projected @@ -105,6 +106,6 @@ def outFile(path, ptsimg): outObj = False if outObj: objdir = "/path/to/objdir/" - outFile(objdir + "reproj_rot_proj.obj", reproject(im2, depthFactor, K)) + outFile(objdir + "reproj_rot_proj.obj", reproject(dstDepth, depthFactor, K)) outFile(objdir + "rotated.obj", reprojectRt(srcDepth, K, depthFactor, rmat, tmat)) From 492d9dba1e354a9833817fb7646a3971278944a0 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 29 Jun 2022 23:51:07 +0200 Subject: [PATCH 11/22] warpFrame() tests big update --- modules/3d/test/test_odometry.cpp | 208 ++++++++++++++++++++++++------ 1 file changed, 168 insertions(+), 40 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index d0f803ac4e..36b1c1d25e 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -468,7 +468,22 @@ TEST(RGBD_Odometry_FastICP, prepareFrame) } -void warpFrameTest(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform) +struct WarpFrameTest +{ + WarpFrameTest() : + srcDepth(), srcRgb(), srcMask(), + dstDepth(), dstRgb(), dstMask(), + warpedDepth(), warpedRgb(), warpedMask() + {} + + void run(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform, int depthType, int imageType); + + Mat srcDepth, srcRgb, srcMask; + Mat dstDepth, dstRgb, dstMask; + Mat warpedDepth, warpedRgb, warpedMask; +}; + +void WarpFrameTest::run(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform, int depthType, int rgbType) { std::string dataPath = cvtest::TS::ptr()->get_data_path(); std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png"; @@ -477,8 +492,6 @@ void warpFrameTest(bool needRgb, bool scaleDown, bool checkMask, bool identityTr std::string warpedDepthFilename = dataPath + "/cv/rgbd/warpedDepth.png"; std::string warpedRgbFilename = dataPath + "/cv/rgbd/warpedRgb.png"; - Mat srcDepth, srcRgb, warpedDepth, warpedRgb; - srcDepth = imread(srcDepthFilename, IMREAD_UNCHANGED); ASSERT_FALSE(srcDepth.empty()) << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl; @@ -495,89 +508,204 @@ void warpFrameTest(bool needRgb, bool scaleDown, bool checkMask, bool identityTr ASSERT_TRUE(srcDepth.type() == CV_16UC1); ASSERT_TRUE(warpedDepth.type() == CV_16UC1); + Mat epsSrc = srcDepth > 0, epsWarped = warpedDepth > 0; + + const double depthFactor = 5000.0; + // scale float types only + double depthScaleCoeff = scaleDown ? ( depthType == CV_16U ? 1. : 1./depthFactor ) : 1.; + double transScaleCoeff = scaleDown ? ( depthType == CV_16U ? depthFactor : 1. ) : depthFactor; + + Mat srcDepthCvt, warpedDepthCvt; + srcDepth.convertTo(srcDepthCvt, depthType, depthScaleCoeff); + srcDepth = srcDepthCvt; + warpedDepth.convertTo(warpedDepthCvt, depthType, depthScaleCoeff); + warpedDepth = warpedDepthCvt; + + Scalar badVal; + switch (depthType) + { + case CV_16U: + badVal = 0; + break; + case CV_32F: + badVal = std::numeric_limits::quiet_NaN(); + break; + case CV_64F: + badVal = std::numeric_limits::quiet_NaN(); + break; + default: + CV_Error(Error::StsBadArg, "Unsupported depth data type"); + } + + srcDepth.setTo(badVal, ~epsSrc); + warpedDepth.setTo(badVal, ~epsWarped); + + if (checkMask) + { + srcMask = epsSrc; warpedMask = epsWarped; + } + else + { + srcMask = Mat(); warpedMask = Mat(); + } + if (needRgb) { - srcRgb = imread(srcRgbFilename); + srcRgb = imread(srcRgbFilename, rgbType == CV_8UC1 ? IMREAD_GRAYSCALE : IMREAD_COLOR); ASSERT_FALSE(srcRgb.empty()) << "Image " << srcRgbFilename.c_str() << "can not be read" << std::endl; if (identityTransform) { - warpedRgb = srcRgb; + srcRgb.copyTo(warpedRgb, epsSrc); } else { - warpedRgb = imread(warpedRgbFilename); + warpedRgb = imread(warpedRgbFilename, rgbType == CV_8UC1 ? IMREAD_GRAYSCALE : IMREAD_COLOR); ASSERT_FALSE (warpedRgb.empty()) << "Image " << warpedRgbFilename.c_str() << "can not be read" << std::endl; } - ASSERT_TRUE(warpedRgb.type() == CV_8UC3); + if (rgbType == CV_8UC4) + { + Mat newSrcRgb, newWarpedRgb; + cvtColor(srcRgb, newSrcRgb, COLOR_RGB2RGBA); + srcRgb = newSrcRgb; + // let's keep alpha channel + std::vector warpedRgbChannels; + split(warpedRgb, warpedRgbChannels); + warpedRgbChannels.push_back(epsWarped); + merge(warpedRgbChannels, newWarpedRgb); + warpedRgb = newWarpedRgb; + } + + ASSERT_TRUE(srcRgb.type() == rgbType); + ASSERT_TRUE(warpedRgb.type() == rgbType); } else { srcRgb = Mat(); warpedRgb = Mat(); } + // test data used to generate warped depth and rgb + // the script used to generate is at TODO: here double fx = 525.0, fy = 525.0, cx = 319.5, cy = 239.5; Matx33d K(fx, 0, cx, 0, fy, cy, 0, 0, 1); cv::Affine3d rt; - rt = identityTransform ? cv::Affine3d() : cv::Affine3d(cv::Vec3d(0.1, 0.2, 0.3), cv::Vec3d(-0.04, 0.05, 0.6)); + cv::Vec3d tr(-0.04, 0.05, 0.6); + rt = identityTransform ? cv::Affine3d() : cv::Affine3d(cv::Vec3d(0.1, 0.2, 0.3), tr * transScaleCoeff); - float scaleCoeff = scaleDown ? 1.f/5000.f : 1.f; - Mat srcDepthCvt, warpedDepthCvt; - srcDepth.convertTo(srcDepthCvt, CV_32FC1, scaleCoeff); - srcDepth = srcDepthCvt; - warpedDepth.convertTo(warpedDepthCvt, CV_32FC1, scaleCoeff); - warpedDepth = warpedDepthCvt; + warpFrame(srcDepth, srcRgb, srcMask, rt.matrix, K, dstDepth, dstRgb, dstMask); +} - //TODO: check that NaNs in depth work the same as explicit mask - Mat epsSrc = srcDepth >= FLT_EPSILON, epsWarped = warpedDepth >= FLT_EPSILON; - Mat srcMask, warpedMask; - if (checkMask) - { - srcMask = epsSrc; warpedMask = epsWarped; - } - else + +TEST(RGBD_Odometry_WarpFrame, inputTypes) +{ + // [depthType, rgbType] + std::array types = + { CV_16U, CV_8UC3, + CV_32F, CV_8UC3, + CV_64F, CV_8UC3, + CV_32F, CV_8UC1, + CV_32F, CV_8UC4 }; + const double shortl2diff = 233.983; + const double shortlidiff = 1; + const double floatl2diff = 0.03820836; + const double floatlidiff = 0.00020004; + for (int i = 0; i < 5; i++) { - srcMask = Mat(); warpedMask = Mat(); - } - srcDepth.setTo(std::numeric_limits::quiet_NaN(), ~epsSrc); - warpedDepth.setTo(std::numeric_limits::quiet_NaN(), ~epsWarped); + int depthType = types[i*2 + 0]; + int rgbType = types[i*2 + 1]; + + WarpFrameTest w; + // scale down does not happen on CV_16U + // to avoid integer overflow + w.run(/* needRgb */ true, /* scaleDown*/ true, + /* checkMask */ true, /* identityTransform */ false, depthType, rgbType); + + double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2); + double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2); - Mat distCoeff, dstRgb, dstDepth, dstMask; - warpFrame(srcRgb, srcDepth, srcMask, rt.matrix, K, - dstRgb, dstDepth, dstMask); + EXPECT_EQ(0, maskDiff); + EXPECT_EQ(0, rgbDiff); - //TODO: finish it + double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); + double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); - //TODO: check this norm - double depthDiff = cv::norm(dstDepth, warpedDepth, NORM_L2); - double rgbDiff = cv::norm(dstRgb, warpedRgb, NORM_L2); - double maskDiff = cv::norm(dstMask, warpedMask, NORM_L2); - //TODO: find true threshold, maybe based on pixcount - ASSERT_LE(0.1, depthDiff); + double l2threshold = depthType == CV_16U ? shortl2diff : floatl2diff; + double lithreshold = depthType == CV_16U ? shortlidiff : floatlidiff; + + EXPECT_GE(l2threshold, l2diff); + EXPECT_GE(lithreshold, lidiff); + } } TEST(RGBD_Odometry_WarpFrame, identity) { - warpFrameTest(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ true); + WarpFrameTest w; + w.run(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ true, CV_32F, CV_8UC3); + + double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2); + double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2); + + ASSERT_EQ(0, rgbDiff); + ASSERT_EQ(0, maskDiff); + + double depthDiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.dstMask); + + ASSERT_GE(DBL_EPSILON, depthDiff); } TEST(RGBD_Odometry_WarpFrame, noRgb) { - warpFrameTest(/* needRgb */ false, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ false); + WarpFrameTest w; + w.run(/* needRgb */ false, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ false, CV_32F, CV_8UC3); + + double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2); + ASSERT_EQ(0, maskDiff); + + double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); + double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); + + ASSERT_GE(0.03820836, l2diff); + ASSERT_GE(0.00020004, lidiff); } TEST(RGBD_Odometry_WarpFrame, nansAreMasked) { - warpFrameTest(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ false, /* identityTransform */ false); + WarpFrameTest w; + w.run(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ false, /* identityTransform */ false, CV_32F, CV_8UC3); + + double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2); + + ASSERT_EQ(0, rgbDiff); + + Mat goodVals = (w.warpedDepth == w.warpedDepth); + + double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, goodVals); + double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, goodVals); + + ASSERT_GE(0.03820836, l2diff); + ASSERT_GE(0.00020004, lidiff); } TEST(RGBD_Odometry_WarpFrame, bigScale) { - warpFrameTest(/* needRgb */ true, /* scaleDown*/ false, /* checkMask */ true, /* identityTransform */ false); + WarpFrameTest w; + w.run(/* needRgb */ true, /* scaleDown*/ false, /* checkMask */ true, /* identityTransform */ false, CV_32F, CV_8UC3); + + double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2); + double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2); + + ASSERT_EQ(0, maskDiff); + ASSERT_EQ(0, rgbDiff); + + double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); + double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); + + ASSERT_GE(191.026565, l2diff); + ASSERT_GE(0.99951172, lidiff); } }} // namespace From fd14b959cf905978a0876883b8e9a54af81cea02 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Wed, 29 Jun 2022 23:55:07 +0200 Subject: [PATCH 12/22] warpFrame() rewritten, interface changed --- modules/3d/include/opencv2/3d/depth.hpp | 26 +-- modules/3d/src/rgbd/odometry.cpp | 255 ++++++++++++++++++------ 2 files changed, 204 insertions(+), 77 deletions(-) diff --git a/modules/3d/include/opencv2/3d/depth.hpp b/modules/3d/include/opencv2/3d/depth.hpp index c0150384d4..0279a26f03 100644 --- a/modules/3d/include/opencv2/3d/depth.hpp +++ b/modules/3d/include/opencv2/3d/depth.hpp @@ -124,20 +124,20 @@ CV_EXPORTS_W void depthTo3d(InputArray depth, InputArray K, OutputArray points3d */ CV_EXPORTS_W void rescaleDepth(InputArray in, int type, OutputArray out, double depth_factor = 1000.0); -/** Warp the image: compute 3d points from the depth, transform them using given transformation, - * then project color point cloud to an image plane. - * This function can be used to visualize results of the Odometry algorithm. - * @param image The image (of CV_8UC1 or CV_8UC3 type) - * @param depth The depth (of type used in depthTo3d fuction) - * @param mask The mask of used pixels (of CV_8UC1), it can be empty - * @param Rt The transformation that will be applied to the 3d points computed from the depth - * @param cameraMatrix Camera matrix - * @param warpedImage The warped image. - * @param warpedDepth The warped depth. - * @param warpedMask The warped mask. +/** Warps depth or RGB-D image by reprojecting it in 3d, applying Rt transformation + * and then projecting it back onto the image plane. + * This function can be used to visualize the results of the Odometry algorithm. + * @param depth Depth data, should be 1-channel CV_16U, CV_16S, CV_32F or CV_64F + * @param image RGB image (optional), should be 1-, 3- or 4-channel CV_8U + * @param mask Mask of used pixels (optional), should be CV_8UC1 + * @param Rt Rotation+translation matrix (3x4 or 4x4) to be applied to depth points + * @param cameraMatrix Camera intrinsics matrix (3x3) + * @param warpedDepth The warped depth data (optional) + * @param warpedImage The warped RGB image (optional) + * @param warpedMask The mask of valid pixels in warped image (optional) */ -CV_EXPORTS_W void warpFrame(InputArray image, InputArray depth, InputArray mask, InputArray Rt, InputArray cameraMatrix, - OutputArray warpedImage, OutputArray warpedDepth = noArray(), OutputArray warpedMask = noArray()); +CV_EXPORTS_W void warpFrame(InputArray depth, InputArray image, InputArray mask, InputArray Rt, InputArray cameraMatrix, + OutputArray warpedDepth = noArray(), OutputArray warpedImage = noArray(), OutputArray warpedMask = noArray()); enum RgbdPlaneMethod { diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index a8ae22721b..ba9fcd57c5 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -362,92 +362,219 @@ bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, } -template -static void -warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, - const Mat& Rt, const Mat& cameraMatrix, - OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask) -{ - //TODO: take into account that image can be empty - //TODO: mask can also be empty - CV_Assert(_image.size() == depth.size()); +void warpFrame(InputArray depth, InputArray image, InputArray mask, + InputArray Rt, InputArray cameraMatrix, + OutputArray warpedDepth, OutputArray warpedImage, OutputArray warpedMask) +{ + CV_Assert(cameraMatrix.size() == Size(3, 3)); + CV_Assert(cameraMatrix.depth() == CV_32F || cameraMatrix.depth() == CV_64F); + Matx33d K, Kinv; + cameraMatrix.getMat().convertTo(K, CV_64F); + std::vector camPlaces { /* fx */ true, false, /* cx */ true, false, /* fy */ true, /* cy */ true, false, false, /* 1 */ true}; + for (int i = 0; i < 9; i++) + { + CV_Assert(camPlaces[i] == (K.val[i] > DBL_EPSILON)); + } + Kinv = K.inv(); + + CV_Assert((Rt.cols() == 4) && (Rt.rows() == 3 || Rt.rows() == 4)); + CV_Assert(Rt.depth() == CV_32F || Rt.depth() == CV_64F); + Mat rtmat; + Rt.getMat().convertTo(rtmat, CV_64F); + Affine3d rt(rtmat); + + CV_Assert(!depth.empty()); + CV_Assert(depth.channels() == 1); + double maxDepth = 0; + int depthDepth = depth.depth(); + switch (depthDepth) + { + case CV_16U: + maxDepth = std::numeric_limits::max(); + break; + case CV_32F: + maxDepth = std::numeric_limits::max(); + break; + case CV_64F: + maxDepth = std::numeric_limits::max(); + break; + default: + CV_Error(Error::StsBadArg, "Unsupported depth data type"); + } + Mat_ depthDbl; + depth.getMat().convertTo(depthDbl, CV_64F); + Size sz = depth.size(); - Mat cloud; - depthTo3d(depth, cameraMatrix, cloud); + Mat_ maskMat; + if (!mask.empty()) + { + CV_Assert(mask.type() == CV_8UC1); + CV_Assert(mask.size() == sz); + maskMat = mask.getMat(); + } - //TODO: replace this by channel split/merge after the function is covered by tests - Mat cloud3; - cloud3.create(cloud.size(), CV_32FC3); - for (int y = 0; y < cloud3.rows; y++) + int imageType = -1; + Mat imageMat; + if (!image.empty()) { - for (int x = 0; x < cloud3.cols; x++) + imageType = image.type(); + CV_Assert(imageType == CV_8UC1 || imageType == CV_8UC3 || imageType == CV_8UC4); + CV_Assert(image.size() == sz); + CV_Assert(warpedImage.needed()); + imageMat = image.getMat(); + } + + CV_Assert(warpedDepth.needed() || warpedImage.needed() || warpedMask.needed()); + + // Getting new coords for depth point + + // see the explanation in the loop below + Matx33d krki = K * rt.rotation() * Kinv; + Matx32d krki_cols01 = krki.get_minor<3, 2>(0, 0); + Vec3d krki_col2(krki.col(2).val); + + Vec3d ktmat = K * rt.translation(); + Mat_ reprojBack(depth.size()); + for (int y = 0; y < sz.height; y++) + { + const uchar* maskRow = maskMat.empty() ? nullptr : maskMat[y]; + const double* depthRow = depthDbl[y]; + Vec3d* reprojRow = reprojBack[y]; + for (int x = 0; x < sz.width; x++) { - Vec4f p = cloud.at(y, x); - cloud3.at(y, x) = Vec3f(p[0], p[1], p[2]); + double z = depthRow[x]; + bool badz = cvIsNaN(z) || cvIsInf(z) || z <= 0 || z >= maxDepth || (maskRow && !maskRow[x]); + Vec3d v; + if (!badz) + { + // Reproject pixel (x, y) using known z, rotate+translate and project back + // getting new pixel in projective coordinates: + // v = K * Rt * K^-1 * ([x, y, 1] * z) = [new_x*new_z, new_y*new_z, new_z] + // v = K * (R * K^-1 * ([x, y, 1] * z) + t) = + // v = krki * [x, y, 1] * z + ktmat = + // v = (krki_cols01 * [x, y] + krki_col2) * z + K * t + v = (krki_cols01 * Vec2d(x, y) + krki_col2) * z + ktmat; + } + else + { + v = Vec3d(); + } + reprojRow[x] = v; } } - //TODO: do the following instead of the functions: K*R*Kinv*[uv1]*z + K*t - //TODO: optimize it using K's structure - std::vector points2d; - Mat transformedCloud; - perspectiveTransform(cloud3, transformedCloud, Rt); - projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix, - Mat::zeros(1, 5, CV_64FC1), points2d); - - Mat image = _image.getMat(); - Size sz = _image.size(); - Mat mask = _mask.getMat(); - _warpedImage.create(sz, image.type()); - Mat warpedImage = _warpedImage.getMat(); - - Mat zBuffer(sz, CV_32FC1, std::numeric_limits::max()); + // Draw new depth in z-buffer manner + + Mat warpedImageMat; + if (warpedImage.needed()) + { + warpedImage.create(sz, imageType); + warpedImage.setZero(); + warpedImageMat = warpedImage.getMat(); + } + + const double infinity = std::numeric_limits::max(); + + Mat zBuffer(sz, CV_32FC1, infinity); + const Rect rect = Rect(Point(), sz); for (int y = 0; y < sz.height; y++) { - //const Point3f* cloud_row = cloud.ptr(y); - const Point3f* transformedCloud_row = transformedCloud.ptr(y); - const Point2f* points2d_row = &points2d[y * sz.width]; - const ImageElemType* image_row = image.ptr(y); - const uchar* mask_row = mask.empty() ? 0 : mask.ptr(y); + uchar* imageRow1ch = nullptr; + Vec3b* imageRow3ch = nullptr; + Vec4b* imageRow4ch = nullptr; + switch (imageType) + { + case -1: + break; + case CV_8UC1: + imageRow1ch = imageMat.ptr(y); + break; + case CV_8UC3: + imageRow3ch = imageMat.ptr(y); + break; + case CV_8UC4: + imageRow4ch = imageMat.ptr(y); + break; + default: + break; + } + + const Vec3d* reprojRow = reprojBack[y]; for (int x = 0; x < sz.width; x++) { - const float transformed_z = transformedCloud_row[x].z; - const Point2i p2d = points2d_row[x]; - if ((!mask_row || mask_row[x]) && transformed_z > 0 && - rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at(p2d) > transformed_z) + Vec3d v = reprojRow[x]; + double z = v[2]; + + if (z > 0) { - warpedImage.at(p2d) = image_row[x]; - zBuffer.at(p2d) = transformed_z; + Point uv(cvFloor(v[0] / z), cvFloor(v[1] / z)); + if (rect.contains(uv)) + { + float oldz = zBuffer.at(uv); + + if (z < oldz) + { + zBuffer.at(uv) = z; + + switch (imageType) + { + case -1: + break; + case CV_8UC1: + warpedImageMat.at(uv) = imageRow1ch[x]; + break; + case CV_8UC3: + warpedImageMat.at(uv) = imageRow3ch[x]; + break; + case CV_8UC4: + warpedImageMat.at(uv) = imageRow4ch[x]; + break; + default: + break; + } + } + } } } } - if (warpedMask.needed()) - Mat(zBuffer != std::numeric_limits::max()).copyTo(warpedMask); - - if (warpedDepth.needed()) + if (warpedDepth.needed() || warpedMask.needed()) { - zBuffer.setTo(std::numeric_limits::quiet_NaN(), zBuffer == std::numeric_limits::max()); - zBuffer.copyTo(warpedDepth); - } -} + Mat goodMask = (zBuffer < infinity); + if (warpedDepth.needed()) + { + warpedDepth.create(sz, depthDepth); -void warpFrame(InputArray image, InputArray depth, InputArray mask, - InputArray Rt, InputArray cameraMatrix, - OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask) -{ - if (image.type() == CV_8UC1) - warpFrameImpl(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), - warpedImage, warpedDepth, warpedMask); - else if (image.type() == CV_8UC3) - warpFrameImpl>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), - warpedImage, warpedDepth, warpedMask); - else - CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3"); + double badVal; + switch (depthDepth) + { + case CV_16U: + badVal = 0; + break; + case CV_32F: + badVal = std::numeric_limits::quiet_NaN(); + break; + case CV_64F: + badVal = std::numeric_limits::quiet_NaN(); + break; + default: + break; + } + + zBuffer.convertTo(warpedDepth, depthDepth); + warpedDepth.setTo(badVal, ~goodMask); + } + + if (warpedMask.needed()) + { + warpedMask.create(sz, CV_8UC1); + goodMask.copyTo(warpedMask); + } + } } } From 94e5ae6043216623b24e29557fdde2079435e6db Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 30 Jun 2022 00:00:47 +0200 Subject: [PATCH 13/22] depth generator moved from main repo to extra --- modules/3d/misc/python/requirements.txt | 3 - modules/3d/misc/python/warp_test.py | 111 ------------------------ 2 files changed, 114 deletions(-) delete mode 100644 modules/3d/misc/python/requirements.txt delete mode 100644 modules/3d/misc/python/warp_test.py diff --git a/modules/3d/misc/python/requirements.txt b/modules/3d/misc/python/requirements.txt deleted file mode 100644 index a4567695c0..0000000000 --- a/modules/3d/misc/python/requirements.txt +++ /dev/null @@ -1,3 +0,0 @@ -numpy>=1.17.4 -scipy>=1.3.3 -PIL>=7.0.0 diff --git a/modules/3d/misc/python/warp_test.py b/modules/3d/misc/python/warp_test.py deleted file mode 100644 index b8dda4fd5f..0000000000 --- a/modules/3d/misc/python/warp_test.py +++ /dev/null @@ -1,111 +0,0 @@ -import numpy as np -from scipy.spatial.transform import Rotation -from PIL import Image -import os - -depthFactor = 5000 -psize = (640, 480) -fx = 525.0 -fy = 525.0 -cx = psize[0]/2-0.5 -cy = psize[1]/2-0.5 -K = np.array([[fx, 0, cx], - [ 0, fy, cy], - [ 0, 0, 1]]) - -# some random transform -rmat = Rotation.from_rotvec(np.array([0.1, 0.2, 0.3])).as_dcm() -tmat = np.array([[-0.04, 0.05, 0.6]]).T -rtmat = np.vstack((np.hstack((rmat, tmat)), np.array([[0, 0, 0, 1]]))) - -testDataPath = os.getenv("OPENCV_TEST_DATA_PATH", default=None) -srcDepth = np.asarray(Image.open(testDataPath + "/cv/rgbd/depth.png")) -srcRgb = np.asarray(Image.open(testDataPath + "/cv/rgbd/rgb.png")) - -def reproject(image, df, K): - Kinv = np.linalg.inv(K) - xsz, ysz = image.shape[1], image.shape[0] - reprojected = np.zeros((ysz, xsz, 3)) - for y in range(ysz): - for x in range(xsz): - z = image[y, x]/df - - v = Kinv @ np.array([x*z, y*z, z]).T - - #xv = (x - cx)/fx * z - #yv = (y - cy)/fy * z - #zv = z - - reprojected[y, x, :] = v[:] - return reprojected - -def reprojectRtProject(image, K, depthFactor, rmat, tmat): - Kinv = np.linalg.inv(K) - xsz, ysz = image.shape[1], image.shape[0] - projected = np.zeros((ysz, xsz, 3)) - for y in range(ysz): - for x in range(xsz): - z = image[y, x]/depthFactor - - v = K @ (rmat @ Kinv @ np.array([x*z, y*z, z]).T + tmat[:, 0]) - - if z > 0: - projected[y, x, :] = v[:] - - return projected - -def reprojectRt(image, K, depthFactor, rmat, tmat): - Kinv = np.linalg.inv(K) - xsz, ysz = image.shape[1], image.shape[0] - rotated = np.zeros((ysz, xsz, 3)) - for y in range(ysz): - for x in range(xsz): - z = image[y, x]/depthFactor - - v = rmat @ Kinv @ np.array([x*z, y*z, z]).T + tmat[:, 0] - - rotated[y, x, :] = v[:] - - return rotated - -# put projected points on a depth map -def splat(projected, maxv, rgb): - xsz, ysz = projected.shape[1], projected.shape[0] - depth = np.full((ysz, xsz), maxv, np.float32) - colors = np.full((ysz, xsz, 3), 0, np.uint8) - for y in range(ysz): - for x in range(xsz): - p = projected[y, x, :] - z = p[2] - if z > 0: - u, v = int(p[0]/z), int(p[1]/z) - okuv = (u >= 0 and v >= 0 and u < xsz and v < ysz) - if okuv and depth[v, u] > z: - depth[v, u] = z - colors[v, u, :] = rgb[y, x, :] - return depth, colors - -maxv = depthFactor -dstDepth, dstRgb = splat(reprojectRtProject(srcDepth, K, depthFactor, rmat, tmat), maxv, srcRgb) -dstDepth[dstDepth >= maxv] = 0 -dstDepth = (dstDepth*depthFactor).astype(np.uint16) - -Image.fromarray(dstDepth).save(testDataPath + "/cv/rgbd/warpedDepth.png") -Image.fromarray(dstRgb ).save(testDataPath + "/cv/rgbd/warpedRgb.png") - -# debug -def outFile(path, ptsimg): - f = open(path, "w") - for y in range(ptsimg.shape[0]): - for x in range(ptsimg.shape[1]): - v = ptsimg[y, x, :] - if v[2] > 0: - f.write(f"v {v[0]} {v[1]} {v[2]}\n") - f.close() - -outObj = False -if outObj: - objdir = "/path/to/objdir/" - outFile(objdir + "reproj_rot_proj.obj", reproject(dstDepth, depthFactor, K)) - outFile(objdir + "rotated.obj", reprojectRt(srcDepth, K, depthFactor, rmat, tmat)) - From 2b767f9bc8b231c6a285ced536c9e1f9ea8bb07d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 30 Jun 2022 16:51:01 +0200 Subject: [PATCH 14/22] whitespace fix --- modules/3d/src/rgbd/odometry.cpp | 2 +- modules/3d/src/rgbd/odometry_functions.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index ba9fcd57c5..4cba5debc3 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -477,7 +477,7 @@ void warpFrame(InputArray depth, InputArray image, InputArray mask, const double infinity = std::numeric_limits::max(); Mat zBuffer(sz, CV_32FC1, infinity); - + const Rect rect = Rect(Point(), sz); for (int y = 0; y < sz.height; y++) diff --git a/modules/3d/src/rgbd/odometry_functions.cpp b/modules/3d/src/rgbd/odometry_functions.cpp index f6d9649c78..6baea1b2bd 100644 --- a/modules/3d/src/rgbd/odometry_functions.cpp +++ b/modules/3d/src/rgbd/odometry_functions.cpp @@ -778,7 +778,7 @@ bool RGBDICPOdometryImpl(OutputArray _Rt, const Mat& initRt, Mat tmp61(6, 1, CV_64FC1, Scalar(0)); if(transformType == OdometryTransformType::ROTATION) - { + { ksi.copyTo(tmp61.rowRange(0,3)); ksi = tmp61; } @@ -834,7 +834,7 @@ void computeCorresps(const Matx33f& _K, const Mat& rt, diffs = Mat(depthDst.size(), CV_32F, Scalar::all(-1)); // src_2d = K * src_3d, src_3d = K_inv * [src_2d | z] - // + // Matx33d K(_K); Matx33d K_inv = K.inv(DECOMP_SVD); From 2ae7438c6b17e2f7833146062c3ca2ef1ffba526 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 30 Jun 2022 17:10:26 +0200 Subject: [PATCH 15/22] odometry tests fixed --- modules/3d/test/test_odometry.cpp | 76 ++----------------------------- 1 file changed, 5 insertions(+), 71 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 36b1c1d25e..0fbe6cb68c 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -6,73 +6,6 @@ namespace opencv_test { namespace { -static -void warpFrame(const Mat& image, const Mat& depth, const Mat& rvec, const Mat& tvec, const Mat& K, - Mat& warpedImage, Mat& warpedDepth) -{ - CV_Assert(!image.empty()); - CV_Assert(image.type() == CV_8UC1); - - CV_Assert(depth.size() == image.size()); - CV_Assert(depth.type() == CV_32FC1); - - CV_Assert(!rvec.empty()); - CV_Assert(rvec.total() == 3); - CV_Assert(rvec.type() == CV_64FC1); - - CV_Assert(!tvec.empty()); - CV_Assert(tvec.size() == Size(1, 3)); - CV_Assert(tvec.type() == CV_64FC1); - - warpedImage.create(image.size(), CV_8UC1); - warpedImage = Scalar(0); - warpedDepth.create(image.size(), CV_32FC1); - warpedDepth = Scalar(FLT_MAX); - - Mat cloud; - depthTo3d(depth, K, cloud); - - Mat cloud3, channels[4]; - cv::split(cloud, channels); - std::vector merged = { channels[0], channels[1], channels[2] }; - cv::merge(merged, cloud3); - - Mat Rt = Mat::eye(4, 4, CV_64FC1); - { - Mat R, dst; - cv::Rodrigues(rvec, R); - - dst = Rt(Rect(0,0,3,3)); - R.copyTo(dst); - - dst = Rt(Rect(3,0,1,3)); - tvec.copyTo(dst); - } - Mat warpedCloud, warpedImagePoints; - perspectiveTransform(cloud3, warpedCloud, Rt); - projectPoints(warpedCloud.reshape(3, 1), Mat(3,1,CV_32FC1, Scalar(0)), Mat(3,1,CV_32FC1, Scalar(0)), K, Mat(1,5,CV_32FC1, Scalar(0)), warpedImagePoints); - warpedImagePoints = warpedImagePoints.reshape(2, cloud.rows); - Rect r(0, 0, image.cols, image.rows); - for(int y = 0; y < cloud.rows; y++) - { - for(int x = 0; x < cloud.cols; x++) - { - Point p = warpedImagePoints.at(y,x); - if(r.contains(p)) - { - float curDepth = warpedDepth.at(p.y, p.x); - float newDepth = warpedCloud.at(y, x).z; - if(newDepth < curDepth && newDepth > 0) - { - warpedImage.at(p.y, p.x) = image.at(y,x); - warpedDepth.at(p.y, p.x) = newDepth; - } - } - } - } - warpedDepth.setTo(std::numeric_limits::quiet_NaN(), warpedDepth > 100); -} - static void dilateFrame(Mat& image, Mat& depth) { @@ -279,11 +212,11 @@ void OdometryTest::run() { Mat rvec, tvec; generateRandomTransformation(rvec, tvec); + Affine3d rt(rvec, tvec); - Mat warpedImage, warpedDepth, scaledDepth; - + Mat warpedImage, warpedDepth; - warpFrame(image, scaledDepth, rvec, tvec, K, warpedImage, warpedDepth); + warpFrame(depth, image, noArray(), rt.matrix, K, warpedDepth, warpedImage); dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping OdometryFrame odfSrc = odometry.createOdometryFrame(); @@ -312,7 +245,8 @@ void OdometryTest::run() imshow("image", image); imshow("warpedImage", warpedImage); Mat resultImage, resultDepth; - warpFrame(image, depth, calcRvec, calcTvec, K, resultImage, resultDepth); + + warpFrame(depth, image, noArray(), calcRt, K, resultDepth, resultImage); imshow("resultImage", resultImage); waitKey(100); } From 3c3eba868a27a11a997c61cf79634b214052f475 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 30 Jun 2022 17:22:58 +0200 Subject: [PATCH 16/22] floatL2 threshold raised a bit --- modules/3d/test/test_odometry.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 0fbe6cb68c..a2771ba0dc 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -545,7 +545,7 @@ TEST(RGBD_Odometry_WarpFrame, inputTypes) CV_32F, CV_8UC4 }; const double shortl2diff = 233.983; const double shortlidiff = 1; - const double floatl2diff = 0.03820836; + const double floatl2diff = 0.038209; const double floatlidiff = 0.00020004; for (int i = 0; i < 5; i++) { @@ -602,7 +602,7 @@ TEST(RGBD_Odometry_WarpFrame, noRgb) double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); - ASSERT_GE(0.03820836, l2diff); + ASSERT_GE(0.038209, l2diff); ASSERT_GE(0.00020004, lidiff); } @@ -620,7 +620,7 @@ TEST(RGBD_Odometry_WarpFrame, nansAreMasked) double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, goodVals); double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, goodVals); - ASSERT_GE(0.03820836, l2diff); + ASSERT_GE(0.038209, l2diff); ASSERT_GE(0.00020004, lidiff); } From 75a8e3e9566b327cf2ee04d709aedb8b58f7ae1d Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Thu, 30 Jun 2022 21:32:41 +0200 Subject: [PATCH 17/22] minor --- modules/3d/test/test_odometry.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index a2771ba0dc..c20eb48d40 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -520,7 +520,8 @@ void WarpFrameTest::run(bool needRgb, bool scaleDown, bool checkMask, bool ident } // test data used to generate warped depth and rgb - // the script used to generate is at TODO: here + // the script used to generate is in opencv_extra repo + // at testdata/cv/rgbd/warped_depth_generator/warp_test.py double fx = 525.0, fy = 525.0, cx = 319.5, cy = 239.5; Matx33d K(fx, 0, cx, From d4e28f27b7a9ee3dbe180a0428552e61f99ed391 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 24 Jul 2022 18:22:48 +0200 Subject: [PATCH 18/22] warpFrame test parametrized --- modules/3d/test/test_odometry.cpp | 57 ++++++++++++++++--------------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index c20eb48d40..2a1d591770 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -534,48 +534,49 @@ void WarpFrameTest::run(bool needRgb, bool scaleDown, bool checkMask, bool ident warpFrame(srcDepth, srcRgb, srcMask, rt.matrix, K, dstDepth, dstRgb, dstMask); } +typedef std::pair WarpFrameInputTypes; +typedef testing::TestWithParam WarpFrameInputs; -TEST(RGBD_Odometry_WarpFrame, inputTypes) +TEST_P(WarpFrameInputs, checkTypes) { - // [depthType, rgbType] - std::array types = - { CV_16U, CV_8UC3, - CV_32F, CV_8UC3, - CV_64F, CV_8UC3, - CV_32F, CV_8UC1, - CV_32F, CV_8UC4 }; const double shortl2diff = 233.983; const double shortlidiff = 1; const double floatl2diff = 0.038209; const double floatlidiff = 0.00020004; - for (int i = 0; i < 5; i++) - { - int depthType = types[i*2 + 0]; - int rgbType = types[i*2 + 1]; - WarpFrameTest w; - // scale down does not happen on CV_16U - // to avoid integer overflow - w.run(/* needRgb */ true, /* scaleDown*/ true, - /* checkMask */ true, /* identityTransform */ false, depthType, rgbType); + int depthType = GetParam().first; + int rgbType = GetParam().second; + + WarpFrameTest w; + // scale down does not happen on CV_16U + // to avoid integer overflow + w.run(/* needRgb */ true, /* scaleDown*/ true, + /* checkMask */ true, /* identityTransform */ false, depthType, rgbType); - double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2); - double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2); + double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2); + double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2); - EXPECT_EQ(0, maskDiff); - EXPECT_EQ(0, rgbDiff); + EXPECT_EQ(0, maskDiff); + EXPECT_EQ(0, rgbDiff); - double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); - double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); + double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); + double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); - double l2threshold = depthType == CV_16U ? shortl2diff : floatl2diff; - double lithreshold = depthType == CV_16U ? shortlidiff : floatlidiff; + double l2threshold = depthType == CV_16U ? shortl2diff : floatl2diff; + double lithreshold = depthType == CV_16U ? shortlidiff : floatlidiff; - EXPECT_GE(l2threshold, l2diff); - EXPECT_GE(lithreshold, lidiff); - } + EXPECT_GE(l2threshold, l2diff); + EXPECT_GE(lithreshold, lidiff); } +INSTANTIATE_TEST_CASE_P(RGBD_Odometry, WarpFrameInputs, ::testing::Values( + WarpFrameInputTypes { CV_16U, CV_8UC3 }, + WarpFrameInputTypes { CV_32F, CV_8UC3 }, + WarpFrameInputTypes { CV_64F, CV_8UC3 }, + WarpFrameInputTypes { CV_32F, CV_8UC1 }, + WarpFrameInputTypes { CV_32F, CV_8UC4 })); + + TEST(RGBD_Odometry_WarpFrame, identity) { WarpFrameTest w; From 7bdacb8098cc96a624fe104ee5f809a8fd18f3ee Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 24 Jul 2022 18:27:35 +0200 Subject: [PATCH 19/22] threshold comparison made more obvious --- modules/3d/test/test_odometry.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 2a1d591770..8f749df7d8 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -565,8 +565,8 @@ TEST_P(WarpFrameInputs, checkTypes) double l2threshold = depthType == CV_16U ? shortl2diff : floatl2diff; double lithreshold = depthType == CV_16U ? shortlidiff : floatlidiff; - EXPECT_GE(l2threshold, l2diff); - EXPECT_GE(lithreshold, lidiff); + EXPECT_LE(l2diff, l2threshold); + EXPECT_LE(lidiff, lithreshold); } INSTANTIATE_TEST_CASE_P(RGBD_Odometry, WarpFrameInputs, ::testing::Values( @@ -590,7 +590,7 @@ TEST(RGBD_Odometry_WarpFrame, identity) double depthDiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.dstMask); - ASSERT_GE(DBL_EPSILON, depthDiff); + ASSERT_LE(depthDiff, DBL_EPSILON); } TEST(RGBD_Odometry_WarpFrame, noRgb) @@ -604,8 +604,8 @@ TEST(RGBD_Odometry_WarpFrame, noRgb) double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); - ASSERT_GE(0.038209, l2diff); - ASSERT_GE(0.00020004, lidiff); + ASSERT_LE(l2diff, 0.038209); + ASSERT_LE(lidiff, 0.00020004); } TEST(RGBD_Odometry_WarpFrame, nansAreMasked) @@ -622,8 +622,8 @@ TEST(RGBD_Odometry_WarpFrame, nansAreMasked) double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, goodVals); double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, goodVals); - ASSERT_GE(0.038209, l2diff); - ASSERT_GE(0.00020004, lidiff); + ASSERT_LE(l2diff, 0.038209); + ASSERT_LE(lidiff, 0.00020004); } TEST(RGBD_Odometry_WarpFrame, bigScale) @@ -640,8 +640,8 @@ TEST(RGBD_Odometry_WarpFrame, bigScale) double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask); double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask); - ASSERT_GE(191.026565, l2diff); - ASSERT_GE(0.99951172, lidiff); + ASSERT_LE(l2diff, 191.026565); + ASSERT_LE(lidiff, 0.99951172); } }} // namespace From 0b34d90dfaff73491e0971f7251d21d1cf274ca0 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 24 Jul 2022 18:28:26 +0200 Subject: [PATCH 20/22] minor --- modules/3d/test/test_odometry.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 8f749df7d8..2ddcfdbf27 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -186,8 +186,7 @@ void OdometryTest::run() Mat mask(image.size(), CV_8UC1, Scalar(255)); odometry.prepareFrame(odf); - bool isComputed; - isComputed = odometry.compute(odf, odf, calcRt); + bool isComputed = odometry.compute(odf, odf, calcRt); if(!isComputed) { From 02d864070a993040efcc5cce0862325e39e8c2c6 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 24 Jul 2022 18:29:34 +0200 Subject: [PATCH 21/22] minor --- modules/3d/test/test_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 2ddcfdbf27..196aa48c90 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -421,7 +421,7 @@ void WarpFrameTest::run(bool needRgb, bool scaleDown, bool checkMask, bool ident std::string dataPath = cvtest::TS::ptr()->get_data_path(); std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png"; std::string srcRgbFilename = dataPath + "/cv/rgbd/rgb.png"; - // The depth was generated using the script at 3d/misc/python/warp_test.py + // The depth was generated using the script at testdata/cv/rgbd/warped_depth_generator/warp_test.py std::string warpedDepthFilename = dataPath + "/cv/rgbd/warpedDepth.png"; std::string warpedRgbFilename = dataPath + "/cv/rgbd/warpedRgb.png"; From 869123d6f95ba1341c5ea5f6d5bf24e6e4e856a6 Mon Sep 17 00:00:00 2001 From: Rostislav Vasilikhin Date: Sun, 24 Jul 2022 18:40:55 +0200 Subject: [PATCH 22/22] "FAIL() <<" replaced by "ASSERT_*() <<" --- modules/3d/test/test_odometry.cpp | 26 +++++--------------------- 1 file changed, 5 insertions(+), 21 deletions(-) diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 196aa48c90..bd9fa38d35 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -100,14 +100,8 @@ void OdometryTest::readData(Mat& image, Mat& depth) const image = imread(imageFilename, 0); depth = imread(depthFilename, -1); - if(image.empty()) - { - FAIL() << "Image " << imageFilename.c_str() << " can not be read" << std::endl; - } - if(depth.empty()) - { - FAIL() << "Depth " << depthFilename.c_str() << "can not be read" << std::endl; - } + ASSERT_FALSE(image.empty()) << "Image " << imageFilename.c_str() << " can not be read" << std::endl; + ASSERT_FALSE(depth.empty()) << "Depth " << depthFilename.c_str() << "can not be read" << std::endl; CV_DbgAssert(image.type() == CV_8UC1); CV_DbgAssert(depth.type() == CV_16UC1); @@ -161,11 +155,7 @@ void OdometryTest::checkUMats() bool isComputed = odometry.compute(odf, odf, calcRt); ASSERT_TRUE(isComputed); double diff = cv::norm(calcRt, Mat::eye(4, 4, CV_64FC1)); - if (diff > idError) - { - FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << diff << std::endl; - } - + ASSERT_FALSE(diff > idError) << "Incorrect transformation between the same frame (not the identity matrix), diff = " << diff << std::endl; } void OdometryTest::run() @@ -188,15 +178,9 @@ void OdometryTest::run() odometry.prepareFrame(odf); bool isComputed = odometry.compute(odf, odf, calcRt); - if(!isComputed) - { - FAIL() << "Can not find Rt between the same frame" << std::endl; - } + ASSERT_TRUE(isComputed) << "Can not find Rt between the same frame" << std::endl; double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1)); - if (ndiff > idError) - { - FAIL() << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << std::endl; - } + ASSERT_FALSE(ndiff > idError) << "Incorrect transformation between the same frame (not the identity matrix), diff = " << ndiff << std::endl; // 2. Generate random rigid body motion in some ranges several times (iterCount). // On each iteration an input frame is warped using generated transformation.