Merge pull request #21439 from DumDereDum:python_odometry

Odometry python support

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> impl;

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()

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<OdometryICP>(settings, OdometryAlgoType::COMMON);
 }
 
+Odometry::Odometry(OdometryType otype)
+{
+    OdometrySettings settings;
+    switch (otype)
+    {
+    case OdometryType::DEPTH:
+        this->impl = makePtr<OdometryICP>(settings, OdometryAlgoType::FAST);
+        break;
+    case OdometryType::RGB:
+        this->impl = makePtr<OdometryRGB>(settings, OdometryAlgoType::COMMON);
+        break;
+    case OdometryType::RGB_DEPTH:
+        this->impl = makePtr<OdometryRGBD>(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<class ImageElemType>
 static void

modules/3d/src/rgbd/odometry_frame_impl.cpp

@@ -238,12 +238,13 @@ void OdometryFrameImplTMat<TMat>::getPyramidAt(OutputArray _img, OdometryFramePy
 template<typename TMat>
 void OdometryFrameImplTMat<TMat>::findMask(InputArray _depth)
 {
-    Mat d = _depth.getMat();
+    Mat depth_value = _depth.getMat();
-    Mat m(d.size(), CV_8UC1, Scalar(255));
+    CV_Assert(depth_value.type() == DEPTH_TYPE);
-    for (int y = 0; y < d.rows; y++)
+    Mat m(depth_value.size(), CV_8UC1, Scalar(255));
-        for (int x = 0; x < d.cols; x++)
+    for (int y = 0; y < depth_value.rows; y++)
+        for (int x = 0; x < depth_value.cols; x++)
         {
-            if (cvIsNaN(d.at<float>(y, x)) || d.at<float>(y, x) <= FLT_EPSILON)
+            if (cvIsNaN(depth_value.at<float>(y, x)) || depth_value.at<float>(y, x) <= FLT_EPSILON)
                 m.at<uchar>(y, x) = 0;
         }
     this->setMask(m);

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(srcFrame, settings, useDepth);
-    prepareRGBFrameBase(dstFrame, settings);
+    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 (useDepth)
-    if (depth.empty())
     {
-        if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
+        frame.getDepth(depth);
-        {
+        if (depth.empty())
-            TMat pyr0;
-            frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0);
-            frame.setDepth(pyr0);
-        }
-        else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0)
         {
-            TMat cloud;
+            if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
-            frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0);
+            {
-            std::vector<TMat> xyz;
+                TMat pyr0;
-            split(cloud, xyz);
+                frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0);
-            frame.setDepth(xyz[2]);
+                frame.setDepth(pyr0);
+            }
+            else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0)
+            {
+                TMat cloud;
+                frame.getPyramidAt(cloud, OdometryFramePyramidType::PYR_CLOUD, 0);
+                std::vector<TMat> xyz;
+                split(cloud, xyz);
+                frame.setDepth(xyz[2]);
+            }
+            else
+                CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
         }
-        else
+        checkDepth(depth, image.size());
-            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
     }
-    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<TMat> mpyramids;
     std::vector<TMat> npyramids;
-    preparePyramidMask<TMat>(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(),
+    preparePyramidMask<TMat>(mask, dpyramids, settings.getMinDepth(), settings.getMaxDepth(), npyramids, mpyramids);
-        npyramids, mpyramids);
     setPyramids(frame, OdometryFramePyramidType::PYR_MASK, mpyramids);
 }
 

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);
 

modules/3d/src/rgbd/odometry_settings_impl.cpp

@@ -108,10 +108,13 @@ private:
 public:
     class DefaultSets {
     public:
-        const cv::Matx33f defaultCameraMatrix =
+        static const int width  = 640;
-        { /* fx, 0, cx*/ 0, 0, 0,
+        static const int height = 480;
-            /* 0, fy, cy*/ 0, 0, 0,
+        static constexpr float fx = 525.f;
-            /* 0,  0,  0*/ 0, 0, 0 };
+        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<int> defaultIterCounts = { 7, 7, 7, 10 };
 
         static constexpr float defaultMinDepth = 0.f;

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));

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()