diff --git a/modules/3d/include/opencv2/3d/odometry.hpp b/modules/3d/include/opencv2/3d/odometry.hpp index 61e02f45c8..8bd112c2b2 100644 --- a/modules/3d/include/opencv2/3d/odometry.hpp +++ b/modules/3d/include/opencv2/3d/odometry.hpp @@ -18,7 +18,7 @@ namespace cv * @param RGB only rgb image * @param RGB_DEPTH only depth and rgb data simultaneously */ -enum class OdometryType +enum OdometryType { DEPTH = 0, RGB = 1, @@ -32,13 +32,14 @@ enum class OdometryType enum class OdometryAlgoType { COMMON = 0, - FAST = 1 + FAST = 1 }; class CV_EXPORTS_W Odometry { public: - Odometry(); + CV_WRAP Odometry(); + CV_WRAP Odometry(OdometryType otype); Odometry(OdometryType otype, const OdometrySettings settings, OdometryAlgoType algtype); ~Odometry(); @@ -72,6 +73,9 @@ public: */ bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt); + CV_WRAP bool compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const; + CV_WRAP bool compute(InputArray srcDepthFrame, InputArray srcRGBFrame, InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const; + class Impl; private: Ptr impl; diff --git a/modules/3d/misc/python/test/test_odometry.py b/modules/3d/misc/python/test/test_odometry.py new file mode 100644 index 0000000000..cc16725d20 --- /dev/null +++ b/modules/3d/misc/python/test/test_odometry.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python + +import numpy as np +import cv2 as cv + +from tests_common import NewOpenCVTests + +class odometry_test(NewOpenCVTests): + def test_OdometryDefault(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)) + + odometry = cv.Odometry() + warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) + isCorrect = odometry.compute(depth, warped_depth, Rt_res) + + res = np.absolute(Rt_curr - Rt_res).sum() + eps = 0.15 + self.assertLessEqual(res, eps) + self.assertTrue(isCorrect) + + def test_OdometryDepth(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)) + + odometry = cv.Odometry(cv.DEPTH) + warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) + + isCorrect = odometry.compute(depth, warped_depth, Rt_res) + res = np.absolute(Rt_curr - Rt_res).sum() + eps = 0.15 + self.assertLessEqual(res, eps) + self.assertTrue(isCorrect) + + def test_OdometryRGB(self): + rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR) + 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)) + + odometry = cv.Odometry(cv.RGB) + warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480)) + isCorrect = odometry.compute(rgb, warped_rgb, Rt_res) + + res = np.absolute(Rt_curr - Rt_res).sum() + eps = 0.15 + self.assertLessEqual(res, eps) + self.assertTrue(isCorrect) + + def test_OdometryRGB_Depth(self): + depth = self.get_sample('cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH).astype(np.float32) + rgb = self.get_sample('cv/rgbd/rgb.png', cv.IMREAD_ANYCOLOR) + 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)) + + odometry = cv.Odometry(cv.RGB_DEPTH) + warped_depth = cv.warpPerspective(depth, Rt_warp, (640, 480)) + warped_rgb = cv.warpPerspective(rgb, Rt_warp, (640, 480)) + isCorrect = odometry.compute(depth, rgb, warped_depth, warped_rgb, Rt_res) + + res = np.absolute(Rt_curr - Rt_res).sum() + eps = 0.15 + self.assertLessEqual(res, 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 9705b9a7e0..34ef6ed57f 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -21,6 +21,9 @@ public: 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(InputArray srcDepthFrame, InputArray srcRGBFrame, + InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const = 0; }; @@ -38,6 +41,9 @@ public: 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(InputArray srcDepthFrame, InputArray srcRGBFrame, + InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; }; OdometryICP::OdometryICP(OdometrySettings _settings, OdometryAlgoType _algtype) @@ -86,6 +92,23 @@ bool OdometryICP::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds return isCorrect; } +bool OdometryICP::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const +{ + OdometryFrame srcFrame = this->createOdometryFrame(); + OdometryFrame dstFrame = this->createOdometryFrame(); + srcFrame.setDepth(_srcFrame); + dstFrame.setDepth(_dstFrame); + + prepareICPFrame(srcFrame, dstFrame, this->settings, this->algtype); + + bool isCorrect = compute(srcFrame, dstFrame, Rt); + 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"); +} class OdometryRGB : public Odometry::Impl { @@ -101,6 +124,9 @@ public: 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(InputArray srcDepthFrame, InputArray srcRGBFrame, + InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; }; OdometryRGB::OdometryRGB(OdometrySettings _settings, OdometryAlgoType _algtype) @@ -120,12 +146,12 @@ OdometryFrame OdometryRGB::createOdometryFrame() const void OdometryRGB::prepareFrame(OdometryFrame& frame) { - prepareRGBFrame(frame, frame, this->settings); + prepareRGBFrame(frame, frame, this->settings, false); } void OdometryRGB::prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) { - prepareRGBFrame(srcFrame, dstFrame, this->settings); + prepareRGBFrame(srcFrame, dstFrame, this->settings, false); } bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const @@ -145,7 +171,23 @@ bool OdometryRGB::compute(const OdometryFrame& srcFrame, const OdometryFrame& ds OdometryType::RGB, OdometryTransformType::RIGID_TRANSFORMATION, this->algtype); return isCorrect; } +bool OdometryRGB::compute(InputArray _srcFrame, InputArray _dstFrame, OutputArray Rt) const +{ + OdometryFrame srcFrame = this->createOdometryFrame(); + OdometryFrame dstFrame = this->createOdometryFrame(); + srcFrame.setImage(_srcFrame); + dstFrame.setImage(_dstFrame); + + prepareRGBFrame(srcFrame, dstFrame, this->settings, false); + + 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"); +} class OdometryRGBD : public Odometry::Impl { @@ -161,6 +203,9 @@ public: 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(InputArray srcDepthFrame, InputArray srcRGBFrame, + InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const override; }; OdometryRGBD::OdometryRGBD(OdometrySettings _settings, OdometryAlgoType _algtype) @@ -205,6 +250,26 @@ bool OdometryRGBD::compute(const OdometryFrame& srcFrame, const OdometryFrame& d return isCorrect; } +bool OdometryRGBD::compute(InputArray, InputArray, OutputArray) 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 +{ + OdometryFrame srcFrame = this->createOdometryFrame(); + OdometryFrame dstFrame = this->createOdometryFrame(); + srcFrame.setDepth(_srcDepthFrame); + srcFrame.setImage(_srcRGBFrame); + dstFrame.setDepth(_dstDepthFrame); + dstFrame.setImage(_dstRGBFrame); + + prepareRGBDFrame(srcFrame, dstFrame, this->settings, this->algtype); + + bool isCorrect = compute(srcFrame, dstFrame, Rt); + return isCorrect; +} Odometry::Odometry() { @@ -212,6 +277,27 @@ Odometry::Odometry() this->impl = makePtr(settings, OdometryAlgoType::COMMON); } +Odometry::Odometry(OdometryType otype) +{ + OdometrySettings settings; + switch (otype) + { + case OdometryType::DEPTH: + this->impl = makePtr(settings, OdometryAlgoType::FAST); + break; + case OdometryType::RGB: + this->impl = makePtr(settings, OdometryAlgoType::COMMON); + break; + case OdometryType::RGB_DEPTH: + this->impl = makePtr(settings, OdometryAlgoType::COMMON); + break; + default: + CV_Error(Error::StsInternal, + "Incorrect OdometryType, you are able to use only { DEPTH = 0, RGB = 1, RGB_DEPTH = 2 }"); + break; + } +} + Odometry::Odometry(OdometryType otype, OdometrySettings settings, OdometryAlgoType algtype) { switch (otype) @@ -263,6 +349,17 @@ bool Odometry::compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFr return this->impl->compute(srcFrame, dstFrame, Rt); } +bool Odometry::compute(InputArray srcFrame, InputArray dstFrame, OutputArray Rt) const +{ + + return this->impl->compute(srcFrame, dstFrame, Rt); +} +bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, + InputArray dstDepthFrame, InputArray dstRGBFrame, OutputArray Rt) const +{ + return this->impl->compute(srcDepthFrame, srcRGBFrame, dstDepthFrame, dstRGBFrame, Rt); +} + template static void diff --git a/modules/3d/src/rgbd/odometry_frame_impl.cpp b/modules/3d/src/rgbd/odometry_frame_impl.cpp index 234839eddd..6088c64b91 100644 --- a/modules/3d/src/rgbd/odometry_frame_impl.cpp +++ b/modules/3d/src/rgbd/odometry_frame_impl.cpp @@ -238,12 +238,13 @@ void OdometryFrameImplTMat::getPyramidAt(OutputArray _img, OdometryFramePy template void OdometryFrameImplTMat::findMask(InputArray _depth) { - Mat d = _depth.getMat(); - Mat m(d.size(), CV_8UC1, Scalar(255)); - for (int y = 0; y < d.rows; y++) - for (int x = 0; x < d.cols; x++) + Mat depth_value = _depth.getMat(); + CV_Assert(depth_value.type() == DEPTH_TYPE); + Mat m(depth_value.size(), CV_8UC1, Scalar(255)); + for (int y = 0; y < depth_value.rows; y++) + for (int x = 0; x < depth_value.cols; x++) { - if (cvIsNaN(d.at(y, x)) || d.at(y, x) <= FLT_EPSILON) + if (cvIsNaN(depth_value.at(y, x)) || depth_value.at(y, x) <= FLT_EPSILON) m.at(y, x) = 0; } this->setMask(m); diff --git a/modules/3d/src/rgbd/odometry_functions.cpp b/modules/3d/src/rgbd/odometry_functions.cpp index a120db484c..c376728c0c 100644 --- a/modules/3d/src/rgbd/odometry_functions.cpp +++ b/modules/3d/src/rgbd/odometry_functions.cpp @@ -24,14 +24,14 @@ enum void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, OdometryAlgoType algtype) { - prepareRGBFrame(srcFrame, dstFrame, settings); + prepareRGBFrame(srcFrame, dstFrame, settings, true); prepareICPFrame(srcFrame, dstFrame, settings, algtype); } -void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings) +void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometrySettings settings, bool useDepth) { - prepareRGBFrameBase(srcFrame, settings); - prepareRGBFrameBase(dstFrame, settings); + prepareRGBFrameBase(srcFrame, settings, useDepth); + prepareRGBFrameBase(dstFrame, settings, useDepth); prepareRGBFrameSrc(srcFrame, settings); prepareRGBFrameDst(dstFrame, settings); @@ -48,7 +48,7 @@ void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, OdometryS prepareICPFrameDst(dstFrame, settings); } -void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings) +void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool useDepth) { // Can be transformed into template argument in the future // when this algorithm supports OCL UMats too @@ -70,29 +70,33 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings) } checkImage(image); - TMat depth; - frame.getDepth(depth); - if (depth.empty()) + if (useDepth) { - if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0) - { - TMat pyr0; - frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0); - frame.setDepth(pyr0); - } - else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0) + frame.getDepth(depth); + if (depth.empty()) { - TMat cloud; - frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0); - std::vector xyz; - split(cloud, xyz); - frame.setDepth(xyz[2]); + if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0) + { + TMat pyr0; + frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0); + frame.setDepth(pyr0); + } + else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0) + { + TMat cloud; + frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0); + std::vector xyz; + split(cloud, xyz); + frame.setDepth(xyz[2]); + } + else + CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set."); } - else - CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set."); + checkDepth(depth, image.size()); } - checkDepth(depth, image.size()); + else + depth = TMat(image.size(), CV_32F, 1); TMat mask; frame.getMask(mask); @@ -120,8 +124,7 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings) std::vector mpyramids; std::vector npyramids; - preparePyramidMask(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(), - npyramids, mpyramids); + preparePyramidMask(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(), npyramids, mpyramids); setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids); } diff --git a/modules/3d/src/rgbd/odometry_functions.hpp b/modules/3d/src/rgbd/odometry_functions.hpp index 763b143cb4..67db149ea9 100644 --- a/modules/3d/src/rgbd/odometry_functions.hpp +++ b/modules/3d/src/rgbd/odometry_functions.hpp @@ -130,10 +130,10 @@ typedef void (*CalcICPEquationCoeffsPtr)(double*, const Point3f&, const Vec3f&); void prepareRGBDFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype); -void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings); +void prepareRGBFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, bool useDepth); void prepareICPFrame(OdometryFrame& srcFrame, OdometryFrame& dstFrame, const OdometrySettings settings, OdometryAlgoType algtype); -void prepareRGBFrameBase(OdometryFrame& frame, const OdometrySettings settings); +void prepareRGBFrameBase(OdometryFrame& frame, const OdometrySettings settings, bool useDepth); void prepareRGBFrameSrc (OdometryFrame& frame, const OdometrySettings settings); void prepareRGBFrameDst (OdometryFrame& frame, const OdometrySettings settings); diff --git a/modules/3d/src/rgbd/odometry_settings_impl.cpp b/modules/3d/src/rgbd/odometry_settings_impl.cpp index efc780f9c1..ea6f2b0403 100644 --- a/modules/3d/src/rgbd/odometry_settings_impl.cpp +++ b/modules/3d/src/rgbd/odometry_settings_impl.cpp @@ -108,10 +108,13 @@ private: public: class DefaultSets { public: - const cv::Matx33f defaultCameraMatrix = - { /* fx, 0, cx*/ 0, 0, 0, - /* 0, fy, cy*/ 0, 0, 0, - /* 0, 0, 0*/ 0, 0, 0 }; + static const int width = 640; + static const int height = 480; + static constexpr float fx = 525.f; + static constexpr float fy = 525.f; + static constexpr float cx = float(width) / 2.f - 0.5f; + static constexpr float cy = float(height) / 2.f - 0.5f; + const cv::Matx33f defaultCameraMatrix = { fx, 0, cx, 0, fy, cy, 0, 0, 1 }; const std::vector defaultIterCounts = { 7, 7, 7, 10 }; static constexpr float defaultMinDepth = 0.f; diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index 395b6fe7fb..f68b84eda4 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/modules/3d/test/test_odometry.cpp @@ -310,7 +310,7 @@ void OdometryTest::run() } // compare rotation - double possibleError = algtype == OdometryAlgoType::COMMON ? 0.09f : 0.015f; + double possibleError = algtype == OdometryAlgoType::COMMON ? 0.11f : 0.015f; Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec)); Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec)); diff --git a/samples/python/odometry.py b/samples/python/odometry.py new file mode 100644 index 0000000000..648a018763 --- /dev/null +++ b/samples/python/odometry.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python + +import numpy as np +import cv2 as cv + +import argparse +import sys + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument( + '--algo', + help="""DEPTH - works with depth, + RGB - works with images, + RGB_DEPTH - works with all, + default - runs all algos""", + default="") + parser.add_argument( + '-src_d', + '--source_depth_frame', + default="") + parser.add_argument( + '-dst_d', + '--destination_depth_frame', + default="") + parser.add_argument( + '-src_rgb', + '--source_rgb_frame', + default="") + parser.add_argument( + '-dst_rgb', + '--destination_rgb_frame', + default="") + + args = parser.parse_args() + + 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) + depth2 = cv.imread(destination_depth_frame, cv.IMREAD_ANYDEPTH).astype(np.float32) + + if args.algo == "RGB_DEPTH" or args.algo == "RGB" or args.algo == "": + source_rgb_frame = cv.samples.findFile(args.source_rgb_frame) + destination_rgb_frame = cv.samples.findFile(args.destination_rgb_frame) + rgb1 = cv.imread(source_rgb_frame, cv.IMREAD_COLOR) + rgb2 = cv.imread(destination_rgb_frame, cv.IMREAD_COLOR) + + if args.algo == "DEPTH" or args.algo == "": + odometry = cv.Odometry(cv.DEPTH) + Rt = np.zeros((4, 4)) + odometry.compute(depth1, depth2, Rt) + print(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) + 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) + + + +if __name__ == '__main__': + main()