diff --git a/modules/3d/include/opencv2/3d/odometry.hpp b/modules/3d/include/opencv2/3d/odometry.hpp index ec8c0ec3b4..dbeba411db 100644 --- a/modules/3d/include/opencv2/3d/odometry.hpp +++ b/modules/3d/include/opencv2/3d/odometry.hpp @@ -68,8 +68,11 @@ public: CV_WRAP bool compute(InputArray srcDepthFrame, InputArray dstDepthFrame, OutputArray Rt) const; CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const; - //TODO: document it - //requires frame size, initialized at prepareFrame stage() + /** + * @brief Get the normals computer object used for normals calculation (if presented). + * The normals computer is generated at first need during prepareFrame when normals are required for the ICP algorithm + * but not presented by a user. Re-generated each time the related settings change or a new frame arrives with the different size. + */ Ptr getNormalsComputer() const; class Impl; diff --git a/modules/3d/include/opencv2/3d/odometry_frame.hpp b/modules/3d/include/opencv2/3d/odometry_frame.hpp index c71f1dda83..1368b5dddb 100644 --- a/modules/3d/include/opencv2/3d/odometry_frame.hpp +++ b/modules/3d/include/opencv2/3d/odometry_frame.hpp @@ -26,25 +26,79 @@ enum OdometryFramePyramidType N_PYRAMIDS }; +/** + * @brief An object that keeps per-frame data for Odometry algorithms from user-provided images to algorithm-specific precalculated data. + * When not empty, it contains a depth image, a mask of valid pixels and a set of pyramids generated from that data. + * An BGR/Gray image and normals are optional. + * OdometryFrame is made to be used together with Odometry class to reuse precalculated data between Rt data calculations. + * A proper way to do that is to call Odometry::prepareFrames() on prev and next frames and then pass them to Odometry::compute() method. + */ class CV_EXPORTS_W OdometryFrame { public: - //TODO: add to docs: check image channels, if 3 or 4 then do cvtColor(BGR(A)2GRAY) + /** + * @brief Construct a new OdometryFrame object. All non-empty images should have the same size. + * + * @param image An BGR or grayscale image (or noArray() if it's not required for used ICP algorithm). + * Should be CV_8UC3 or CV_8C4 if it's BGR image or CV_8UC1 if it's grayscale. If it's BGR then it's converted to grayscale + * image automatically. + * @param depth A depth image, should be CV_8UC1 + * @param mask A user-provided mask of valid pixels, should be CV_8UC1 + * @param normals A user-provided normals to the depth surface, should be CV_32FC4 + */ OdometryFrame(InputArray image = noArray(), InputArray depth = noArray(), InputArray mask = noArray(), InputArray normals = noArray()); ~OdometryFrame() {}; + /** + * @brief Get the original user-provided BGR/Gray image + * + * @param image Output image + */ void getImage(OutputArray image) const; + /** + * @brief Get the gray image generated from the user-provided BGR/Gray image + * + * @param image Output image + */ void getGrayImage(OutputArray image) const; + /** + * @brief Get the original user-provided depth image + * + * @param depth Output image + */ void getDepth(OutputArray depth) const; - void getScaledDepth(OutputArray depth) const; + /** + * @brief Get the depth image generated from the user-provided one after conversion, rescale or filtering for ICP algorithm needs + * + * @param depth Output image + */ + void getProcessedDepth(OutputArray depth) const; + /** + * @brief Get the valid pixels mask generated for the ICP calculations intersected with the user-provided mask + * + * @param mask Output image + */ void getMask(OutputArray mask) const; + /** + * @brief Get the normals image either generated for the ICP calculations or user-provided + * + * @param normals Output image + */ void getNormals(OutputArray normals) const; - //TODO: add docs - // returns amt of levels in pyramids (all of them should have the same amt of levels) or 0 if no pyramids were prepared yet + /** + * @brief Get the amount of levels in pyramids (all of them if not empty should have the same number of levels) + * or 0 if no pyramids were prepared yet + */ size_t getPyramidLevels() const; - //TODO: add docs - // returns empty img if no data in the pyramid or in the pyramid's level + /** + * @brief Get the image generated for the ICP calculations from one of the pyramids specified by pyrType. Returns empty image if + * the pyramid is empty or there's no such pyramid level + * + * @param img Output image + * @param pyrType Type of pyramid + * @param level Level in the pyramid + */ void getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const; class Impl; diff --git a/modules/3d/src/rgbd/odometry_frame_impl.cpp b/modules/3d/src/rgbd/odometry_frame_impl.cpp index 3bde589443..b1026d52a0 100644 --- a/modules/3d/src/rgbd/odometry_frame_impl.cpp +++ b/modules/3d/src/rgbd/odometry_frame_impl.cpp @@ -35,7 +35,7 @@ OdometryFrame::OdometryFrame(InputArray image, InputArray depth, InputArray mask void OdometryFrame::getImage(OutputArray image) const { this->impl->getImage(image); } void OdometryFrame::getGrayImage(OutputArray image) const { this->impl->getGrayImage(image); } void OdometryFrame::getDepth(OutputArray depth) const { this->impl->getDepth(depth); } -void OdometryFrame::getScaledDepth(OutputArray depth) const { this->impl->getScaledDepth(depth); } +void OdometryFrame::getProcessedDepth(OutputArray depth) const { this->impl->getProcessedDepth(depth); } void OdometryFrame::getMask(OutputArray mask) const { this->impl->getMask(mask); } void OdometryFrame::getNormals(OutputArray normals) const { this->impl->getNormals(normals); } @@ -61,7 +61,7 @@ void OdometryFrame::Impl::getDepth(OutputArray _depth) const _depth.assign(this->depth); } -void OdometryFrame::Impl::getScaledDepth(OutputArray _depth) const +void OdometryFrame::Impl::getProcessedDepth(OutputArray _depth) const { _depth.assign(this->scaledDepth); } diff --git a/modules/3d/src/rgbd/odometry_functions.cpp b/modules/3d/src/rgbd/odometry_functions.cpp index c395dee2e0..beb2683cb1 100644 --- a/modules/3d/src/rgbd/odometry_functions.cpp +++ b/modules/3d/src/rgbd/odometry_functions.cpp @@ -262,7 +262,7 @@ static void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings) //TODO: don't use scaled when scale bug is fixed UMat scaledDepth; - frame.getScaledDepth(scaledDepth); + frame.getProcessedDepth(scaledDepth); if (scaledDepth.empty()) { scaledDepth = prepareScaledDepth(frame); @@ -348,7 +348,7 @@ static void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings) { //TODO: don't use scaled when scale bug is fixed UMat scaledDepth; - frame.getScaledDepth(scaledDepth); + frame.getProcessedDepth(scaledDepth); if (scaledDepth.empty()) { scaledDepth = prepareScaledDepth(frame); @@ -410,7 +410,7 @@ static void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings, settings.getCameraMatrix(cameraMatrix); UMat scaledDepth, mask, normals; - frame.getScaledDepth(scaledDepth); + frame.getProcessedDepth(scaledDepth); frame.getMask(mask); frame.getNormals(normals); @@ -439,6 +439,7 @@ static void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings, normalsComputer->apply(c0, normals); frame.impl->normals = normals; } + CV_Assert(normals.type() == CV_32FC4); const std::vector& dpyramids = frame.impl->pyramids[OdometryFramePyramidType::PYR_DEPTH]; diff --git a/modules/3d/src/rgbd/odometry_functions.hpp b/modules/3d/src/rgbd/odometry_functions.hpp index c1fd2ca584..eca32357f7 100644 --- a/modules/3d/src/rgbd/odometry_functions.hpp +++ b/modules/3d/src/rgbd/odometry_functions.hpp @@ -31,16 +31,6 @@ static inline int getTransformDim(OdometryTransformType transformType) } -static inline -void checkNormals(InputArray normals, const Size& depthSize) -{ - if (normals.size() != depthSize) - CV_Error(Error::StsBadSize, "Normals has to have the size equal to the depth size."); - if (normals.type() != CV_32FC3) - CV_Error(Error::StsBadSize, "Normals type has to be CV_32FC3."); -} - - static inline Vec6d calcRgbdEquationCoeffs(double dIdx, double dIdy, const Point3f& p3d, double fx, double fy) { diff --git a/modules/3d/src/rgbd/utils.hpp b/modules/3d/src/rgbd/utils.hpp index cca2e9464f..d935c98055 100644 --- a/modules/3d/src/rgbd/utils.hpp +++ b/modules/3d/src/rgbd/utils.hpp @@ -247,7 +247,7 @@ public: virtual void getImage(OutputArray image) const ; virtual void getGrayImage(OutputArray image) const ; virtual void getDepth(OutputArray depth) const ; - virtual void getScaledDepth(OutputArray depth) const ; + virtual void getProcessedDepth(OutputArray depth) const ; virtual void getMask(OutputArray mask) const ; virtual void getNormals(OutputArray normals) const ; diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index c67aeeaae8..941863a434 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -310,8 +310,7 @@ void OdometryTest::prepareFrameCheck() ASSERT_LE(grayNorm, 0.0); } - //TODO: remove it when scale issue is fixed - odf.getScaledDepth(scaled); + odf.getProcessedDepth(scaled); int scalednz = countNonZero(scaled); EXPECT_EQ(scalednz, depthnz);