From 907b4d70ee46d6aec657ec5e4ede9bc6f442517e Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Tue, 20 Feb 2024 20:35:22 +0100
Subject: [PATCH 01/11] twist computation

---
 modules/rgbd/CMakeLists.txt                 |  2 +-
 modules/rgbd/include/opencv2/rgbd/twist.hpp | 28 +++++++
 modules/rgbd/src/twist.cpp                  | 83 +++++++++++++++++++++
 3 files changed, 112 insertions(+), 1 deletion(-)
 create mode 100644 modules/rgbd/include/opencv2/rgbd/twist.hpp
 create mode 100644 modules/rgbd/src/twist.cpp

diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt
index 79e15624f1f..33325146586 100644
--- a/modules/rgbd/CMakeLists.txt
+++ b/modules/rgbd/CMakeLists.txt
@@ -1,6 +1,6 @@
 set(the_description "RGBD algorithms")
 
-ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
+ocv_define_module(rgbd opencv_core opencv_tracking opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
 
 if(NOT HAVE_EIGEN)
   message(STATUS "rgbd: Eigen support is disabled. Eigen is Required for Posegraph optimization")
diff --git a/modules/rgbd/include/opencv2/rgbd/twist.hpp b/modules/rgbd/include/opencv2/rgbd/twist.hpp
new file mode 100644
index 00000000000..1ce1e2dcee4
--- /dev/null
+++ b/modules/rgbd/include/opencv2/rgbd/twist.hpp
@@ -0,0 +1,28 @@
+#ifndef __OPENCV_RGBD_TWIST_HPP__
+#define __OPENCV_RGBD_TWIST_HPP__
+
+#include "opencv2/video/tracking.hpp"
+
+namespace cv
+{
+namespace rgbd
+{
+class CV_EXPORTS_W Twist
+{
+public:
+    Twist();
+
+    cv::Vec6d compute(const cv::Mat& im0, const cv::Mat& im1, const cv::Mat depths0,
+                      const cv::Mat& K, const double dt);
+
+private:
+    void interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J);
+
+private:
+    Ptr<DenseOpticalFlow> _optflow;
+    Ptr<cv::Mat> _flow;
+};
+} // namespace rgbd
+} // namespace cv
+
+#endif
diff --git a/modules/rgbd/src/twist.cpp b/modules/rgbd/src/twist.cpp
new file mode 100644
index 00000000000..ca3546e4617
--- /dev/null
+++ b/modules/rgbd/src/twist.cpp
@@ -0,0 +1,83 @@
+
+#include "precomp.hpp"
+#include "opencv2/rgbd/twist.hpp"
+
+namespace cv
+{
+namespace rgbd
+{
+
+Twist::Twist()
+{
+    _optflow = DISOpticalFlow::create(DISOpticalFlow::PRESET_MEDIUM);
+    _flow = new cv::Mat();
+}
+
+void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J)
+{
+    CV_Assert(uv.cols == depth.cols);
+    CV_Assert(depth.type() == CV_32F); // Validate depth input type
+
+    J.create(depth.cols * 2, 6, CV_32F);
+    J.setTo(0);
+
+    cv::Mat Kinv;
+    cv::invert(K, Kinv);
+
+    cv::Mat xy(3, 1, CV_32F);
+    cv::Mat Jp(2, 6, CV_32F);
+    for (int i = 0; i < uv.cols; i++)
+    {
+        const float z = depth.at<float>(i);
+        if (cv::abs(z) < 0.001f)
+            continue;
+
+        const cv::Point3f p(uv.at<float>(0, i), uv.at<float>(1, i), 1.0);
+
+        // convert to normalized image-plane coordinates
+        xy = Kinv * cv::Mat(p);
+        float x = xy.at<float>(0);
+        float y = xy.at<float>(1);
+
+        // 2x6 Jacobian for this point
+        Jp = (cv::Mat_<float>(2, 6) << -1 / z, 0.0, x / z, x * y, -(1 + x * x), y, 0.0, -1 / z,
+              y / z, 1 + y * y, -x * y, -x);
+
+        Jp = K(cv::Rect(0, 0, 2, 2)) * Jp;
+
+        // push into Jacobian
+        Jp.copyTo(J(cv::Rect(0, 2 * i, 6, 2)));
+    }
+}
+
+cv::Vec6d Twist::compute(const cv::Mat& im0, const cv::Mat& im1, const cv::Mat depths0, const cv::Mat& K,
+               const double dt)
+{
+    _optflow->calc(im0, im1, *_flow);
+
+    const int N = (im0.cols * im0.rows) * 0.1;
+    cv::Mat uv(2, N, CV_32F);
+    cv::Mat depth(1, N, CV_32F);
+    cv::Mat flow(1, 2 * N, CV_32F);
+    for (int i = 0; i < N; i++)
+    {
+        int x = rand() % im0.cols;
+        int y = rand() % im0.rows;
+        uv.at<float>(0, i) = x;
+        uv.at<float>(1, i) = y;
+        depth.at<float>(i) = depths0.at<float>(y, x);
+        flow.at<float>(0, 2 * i) = _flow->at<cv::Point2f>(y, x).x;
+        flow.at<float>(0, 2 * i + 1) = _flow->at<cv::Point2f>(y, x).y;
+    }
+    cv::Mat J;
+    interactionMatrix(uv, depth, K, J);
+
+    cv::Mat Jinv;
+    cv::invert(J, Jinv, cv::DECOMP_SVD);
+    cv::Mat duv = flow / dt;
+    cv::Mat twist = Jinv * duv.t();
+    return twist;
+}
+
+} // namespace rgbd
+} // namespace cv

From b6e397e69ed78726922dc82325eff39cbea6cd9b Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Thu, 7 Mar 2024 15:59:09 +0100
Subject: [PATCH 02/11] port twist to tracking

---
 modules/rgbd/CMakeLists.txt                   |  2 +-
 .../include/opencv2/tracking}/twist.hpp       | 23 +++++++++++++++----
 modules/{rgbd => tracking}/src/twist.cpp      | 15 ++++++++----
 3 files changed, 29 insertions(+), 11 deletions(-)
 rename modules/{rgbd/include/opencv2/rgbd => tracking/include/opencv2/tracking}/twist.hpp (61%)
 rename modules/{rgbd => tracking}/src/twist.cpp (90%)

diff --git a/modules/rgbd/CMakeLists.txt b/modules/rgbd/CMakeLists.txt
index 33325146586..79e15624f1f 100644
--- a/modules/rgbd/CMakeLists.txt
+++ b/modules/rgbd/CMakeLists.txt
@@ -1,6 +1,6 @@
 set(the_description "RGBD algorithms")
 
-ocv_define_module(rgbd opencv_core opencv_tracking opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
+ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python)
 
 if(NOT HAVE_EIGEN)
   message(STATUS "rgbd: Eigen support is disabled. Eigen is Required for Posegraph optimization")
diff --git a/modules/rgbd/include/opencv2/rgbd/twist.hpp b/modules/tracking/include/opencv2/tracking/twist.hpp
similarity index 61%
rename from modules/rgbd/include/opencv2/rgbd/twist.hpp
rename to modules/tracking/include/opencv2/tracking/twist.hpp
index 1ce1e2dcee4..7c539438c76 100644
--- a/modules/rgbd/include/opencv2/rgbd/twist.hpp
+++ b/modules/tracking/include/opencv2/tracking/twist.hpp
@@ -1,13 +1,22 @@
-#ifndef __OPENCV_RGBD_TWIST_HPP__
-#define __OPENCV_RGBD_TWIST_HPP__
+#ifndef OPENCV_TWIST_HPP
+#define OPENCV_TWIST_HPP
 
 #include "opencv2/video/tracking.hpp"
+#include "opencv2/core.hpp"
 
 namespace cv
 {
-namespace rgbd
+class CV_EXPORTS Twist
+{};
+
+namespace detail
+{
+inline namespace tracking
 {
-class CV_EXPORTS_W Twist
+//! @addtogroup tracking_detail
+//! @{
+
+class CV_EXPORTS Twist
 {
 public:
     Twist();
@@ -22,7 +31,11 @@ class CV_EXPORTS_W Twist
     Ptr<DenseOpticalFlow> _optflow;
     Ptr<cv::Mat> _flow;
 };
-} // namespace rgbd
+
+//! @}
+
+} // namespace tracking
+} // namespace detail
 } // namespace cv
 
 #endif
diff --git a/modules/rgbd/src/twist.cpp b/modules/tracking/src/twist.cpp
similarity index 90%
rename from modules/rgbd/src/twist.cpp
rename to modules/tracking/src/twist.cpp
index ca3546e4617..56aeff90c41 100644
--- a/modules/rgbd/src/twist.cpp
+++ b/modules/tracking/src/twist.cpp
@@ -1,10 +1,12 @@
 
 #include "precomp.hpp"
-#include "opencv2/rgbd/twist.hpp"
+#include "opencv2/tracking/twist.hpp"
 
 namespace cv
 {
-namespace rgbd
+namespace detail
+{
+inline namespace tracking
 {
 
 Twist::Twist()
@@ -13,7 +15,8 @@ Twist::Twist()
     _flow = new cv::Mat();
 }
 
-void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J)
+void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat&
+J)
 {
     CV_Assert(uv.cols == depth.cols);
     CV_Assert(depth.type() == CV_32F); // Validate depth input type
@@ -50,7 +53,8 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
     }
 }
 
-cv::Vec6d Twist::compute(const cv::Mat& im0, const cv::Mat& im1, const cv::Mat depths0, const cv::Mat& K,
+cv::Vec6d Twist::compute(const cv::Mat& im0, const cv::Mat& im1, const cv::Mat depths0, const
+cv::Mat& K,
                const double dt)
 {
     _optflow->calc(im0, im1, *_flow);
@@ -79,5 +83,6 @@ cv::Vec6d Twist::compute(const cv::Mat& im0, const cv::Mat& im1, const cv::Mat d
     return twist;
 }
 
-} // namespace rgbd
+} // namespace tracking
+} // namespace detail
 } // namespace cv

From e7549e0d81f527c05a253d68c69267f76f9b10b2 Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Fri, 8 Mar 2024 12:00:09 +0100
Subject: [PATCH 03/11] change API to take in pixel velocities

---
 .../include/opencv2/tracking/twist.hpp        | 17 ++++-----
 modules/tracking/src/twist.cpp                | 37 +++----------------
 2 files changed, 13 insertions(+), 41 deletions(-)

diff --git a/modules/tracking/include/opencv2/tracking/twist.hpp b/modules/tracking/include/opencv2/tracking/twist.hpp
index 7c539438c76..c7682f91597 100644
--- a/modules/tracking/include/opencv2/tracking/twist.hpp
+++ b/modules/tracking/include/opencv2/tracking/twist.hpp
@@ -1,13 +1,13 @@
 #ifndef OPENCV_TWIST_HPP
 #define OPENCV_TWIST_HPP
 
-#include "opencv2/video/tracking.hpp"
 #include "opencv2/core.hpp"
 
 namespace cv
 {
 class CV_EXPORTS Twist
-{};
+{
+};
 
 namespace detail
 {
@@ -19,17 +19,14 @@ inline namespace tracking
 class CV_EXPORTS Twist
 {
 public:
-    Twist();
+    Twist() = default;
 
-    cv::Vec6d compute(const cv::Mat& im0, const cv::Mat& im1, const cv::Mat depths0,
-                      const cv::Mat& K, const double dt);
+    // TODO(ernie): docs
+    cv::Vec6d compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat depths,
+                      const cv::Mat& K);
 
-private:
+    // TODO(ernie): docs
     void interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J);
-
-private:
-    Ptr<DenseOpticalFlow> _optflow;
-    Ptr<cv::Mat> _flow;
 };
 
 //! @}
diff --git a/modules/tracking/src/twist.cpp b/modules/tracking/src/twist.cpp
index 56aeff90c41..b9b1ecfb0d1 100644
--- a/modules/tracking/src/twist.cpp
+++ b/modules/tracking/src/twist.cpp
@@ -9,14 +9,7 @@ namespace detail
 inline namespace tracking
 {
 
-Twist::Twist()
-{
-    _optflow = DISOpticalFlow::create(DISOpticalFlow::PRESET_MEDIUM);
-    _flow = new cv::Mat();
-}
-
-void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat&
-J)
+void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J)
 {
     CV_Assert(uv.cols == depth.cols);
     CV_Assert(depth.type() == CV_32F); // Validate depth input type
@@ -32,6 +25,7 @@ J)
     for (int i = 0; i < uv.cols; i++)
     {
         const float z = depth.at<float>(i);
+        // skip points with zero depth
         if (cv::abs(z) < 0.001f)
             continue;
 
@@ -53,33 +47,14 @@ J)
     }
 }
 
-cv::Vec6d Twist::compute(const cv::Mat& im0, const cv::Mat& im1, const cv::Mat depths0, const
-cv::Mat& K,
-               const double dt)
+cv::Vec6d Twist::compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat depths,
+                         const cv::Mat& K)
 {
-    _optflow->calc(im0, im1, *_flow);
-
-    const int N = (im0.cols * im0.rows) * 0.1;
-    cv::Mat uv(2, N, CV_32F);
-    cv::Mat depth(1, N, CV_32F);
-    cv::Mat flow(1, 2 * N, CV_32F);
-    for (int i = 0; i < N; i++)
-    {
-        int x = rand() % im0.cols;
-        int y = rand() % im0.rows;
-        uv.at<float>(0, i) = x;
-        uv.at<float>(1, i) = y;
-        depth.at<float>(i) = depths0.at<float>(y, x);
-        flow.at<float>(0, 2 * i) = _flow->at<cv::Point2f>(y, x).x;
-        flow.at<float>(0, 2 * i + 1) = _flow->at<cv::Point2f>(y, x).y;
-    }
     cv::Mat J;
-    interactionMatrix(uv, depth, K, J);
-
+    interactionMatrix(uv, depths, K, J);
     cv::Mat Jinv;
     cv::invert(J, Jinv, cv::DECOMP_SVD);
-    cv::Mat duv = flow / dt;
-    cv::Mat twist = Jinv * duv.t();
+    cv::Mat twist = Jinv * duv.reshape(1, duv.total());
     return twist;
 }
 

From d4c0cd4952bd24c1abbf5bac3072ee9c1320a17d Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Fri, 8 Mar 2024 12:00:24 +0100
Subject: [PATCH 04/11] initial test setup

---
 modules/tracking/test/test_twist.cpp | 42 ++++++++++++++++++++++++++++
 1 file changed, 42 insertions(+)
 create mode 100644 modules/tracking/test/test_twist.cpp

diff --git a/modules/tracking/test/test_twist.cpp b/modules/tracking/test/test_twist.cpp
new file mode 100644
index 00000000000..f2232c65d33
--- /dev/null
+++ b/modules/tracking/test/test_twist.cpp
@@ -0,0 +1,42 @@
+#include "test_precomp.hpp"
+
+#include "opencv2/core.hpp"
+#include "opencv2/tracking/twist.hpp"
+
+namespace opencv_test
+{
+namespace
+{
+
+class TwistTest : public ::testing::Test
+{
+protected:
+    cv::detail::tracking::Twist twist;
+    cv::Mat uv, depth, K, J, duv;
+    cv::Vec6d result;
+
+    void SetUp() override
+    {
+        uv = (cv::Mat_<float>(2, 2) << 0.0, 0.0, 0.0, 0.0);
+        depth = (cv::Mat_<float>(1, 2) << 1.0, 1.0);
+        K = (cv::Mat_<float>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+        duv = (cv::Mat_<float>(2, 2) << 0.0, 0.0, 0.0, 0.0);
+    }
+};
+
+TEST_F(TwistTest, TestInteractionMatrix)
+{
+    twist.interactionMatrix(uv, depth, K, J);
+    ASSERT_EQ(J.cols, 6);
+    ASSERT_EQ(J.rows, 4);
+}
+
+TEST_F(TwistTest, TestCompute)
+{
+    result = twist.compute(uv, duv, depth, K);
+    for (int i = 0; i < 6; i++)
+        ASSERT_NEAR(result[i], 0.0, 1e-6);
+}
+
+} // namespace
+} // namespace opencv_test

From 2a2d6c6ae20e43f58b5cc5480939c327a2db4cbe Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Fri, 8 Mar 2024 13:03:06 +0100
Subject: [PATCH 05/11] test coverage

---
 modules/tracking/src/twist.cpp       |  2 +-
 modules/tracking/test/test_twist.cpp | 75 +++++++++++++++++++++++++---
 2 files changed, 68 insertions(+), 9 deletions(-)

diff --git a/modules/tracking/src/twist.cpp b/modules/tracking/src/twist.cpp
index b9b1ecfb0d1..5494bbdafa6 100644
--- a/modules/tracking/src/twist.cpp
+++ b/modules/tracking/src/twist.cpp
@@ -54,7 +54,7 @@ cv::Vec6d Twist::compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat de
     interactionMatrix(uv, depths, K, J);
     cv::Mat Jinv;
     cv::invert(J, Jinv, cv::DECOMP_SVD);
-    cv::Mat twist = Jinv * duv.reshape(1, duv.total());
+    cv::Mat twist = Jinv * duv;
     return twist;
 }
 
diff --git a/modules/tracking/test/test_twist.cpp b/modules/tracking/test/test_twist.cpp
index f2232c65d33..cac7158f094 100644
--- a/modules/tracking/test/test_twist.cpp
+++ b/modules/tracking/test/test_twist.cpp
@@ -12,31 +12,90 @@ class TwistTest : public ::testing::Test
 {
 protected:
     cv::detail::tracking::Twist twist;
-    cv::Mat uv, depth, K, J, duv;
-    cv::Vec6d result;
+    cv::Mat K, J;
 
     void SetUp() override
     {
-        uv = (cv::Mat_<float>(2, 2) << 0.0, 0.0, 0.0, 0.0);
-        depth = (cv::Mat_<float>(1, 2) << 1.0, 1.0);
         K = (cv::Mat_<float>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
-        duv = (cv::Mat_<float>(2, 2) << 0.0, 0.0, 0.0, 0.0);
     }
 };
 
 TEST_F(TwistTest, TestInteractionMatrix)
 {
+    // import machinevisiontoolbox as mv
+    // cam = mv.CentralCamera()
+    // print(cam.K)
+    // print(cam.visjac_p([1, 1], 2.0))
+    // [[1. 0. 0.]
+    //  [0. 1. 0.]
+    //  [0. 0. 1.]]
+    // [[-0.5  0.   0.5  1.  -2.   1. ]
+    //  [ 0.  -0.5  0.5  2.  -1.  -1. ]]
+
+    cv::Mat uv = (cv::Mat_<float>(2, 1) << 1.0, 1.0);
+    cv::Mat depth = (cv::Mat_<float>(1, 1) << 2.0);
+
     twist.interactionMatrix(uv, depth, K, J);
     ASSERT_EQ(J.cols, 6);
-    ASSERT_EQ(J.rows, 4);
+    ASSERT_EQ(J.rows, 2);
+    float expected[2][6] = {{-0.5, 0.0, 0.5, 1.0, -2.0, 1.0}, {0.0, -0.5, 0.5, 2.0, -1.0, -1.0}};
+    for (int i = 0; i < 2; i++)
+        for (int j = 0; j < 6; j++)
+            ASSERT_NEAR(J.at<float>(i, j), expected[i][j], 1e-6);
 }
 
-TEST_F(TwistTest, TestCompute)
+TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
 {
-    result = twist.compute(uv, duv, depth, K);
+    cv::Mat uv = (cv::Mat_<float>(2, 2) << 0.0, 0.0, 0.0, 0.0);
+    cv::Mat depth = (cv::Mat_<float>(1, 2) << 1.0, 1.0);
+    cv::Mat duv = (cv::Mat_<float>(4, 1) << 0.0, 0.0, 0.0, 0.0);
+
+    cv::Vec6d result = twist.compute(uv, duv, depth, K);
     for (int i = 0; i < 6; i++)
         ASSERT_NEAR(result[i], 0.0, 1e-6);
 }
 
+TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
+{
+    // import machinevisiontoolbox as mv
+    // cam = mv.CentralCamera()
+    // pixels = np.array([[1, 2, 3],
+    //                    [1, 2, 3]], dtype=float)
+    // depths = np.array([1.0, 2.0, 3.0])
+    // Jac = cam.visjac_p(pixels, depths)
+    // duv = np.array([1, 2, 1, 3, 1, 4])
+    // twist = np.linalg.lstsq(Jac, duv, rcond=None)[0]
+    // print(twist)
+    // print(Jac)
+    // [ 0.5       0.5       1.875     0.041667 -0.041667 -0.5     ]
+    // [[ -1.         0.         1.         1.        -2.         1.      ]
+    //  [  0.        -1.         1.         2.        -1.        -1.      ]
+    //  [ -0.5        0.         1.         4.        -5.         2.      ]
+    //  [  0.        -0.5        1.         5.        -4.        -2.      ]
+    //  [ -0.333333   0.         1.         9.       -10.         3.      ]
+    //  [  0.        -0.333333   1.        10.        -9.        -3.      ]]
+
+    cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
+    cv::Mat depth = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
+    cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);
+
+    twist.interactionMatrix(uv, depth, K, J);
+    ASSERT_EQ(J.cols, 6);
+    ASSERT_EQ(J.rows, 6);
+    float expected_jac[6][6] = {
+        {-1.0, 0.0, 1.0, 1.0, -2.0, 1.0},       {0.0, -1.0, 1.0, 2.0, -1.0, -1.0},
+        {-0.5, 0.0, 1.0, 4.0, -5.0, 2.0},       {0.0, -0.5, 1.0, 5.0, -4.0, -2.0},
+        {-0.333333, 0.0, 1.0, 9.0, -10.0, 3.0}, {0.0, -0.333333, 1.0, 10.0, -9.0, -3.0}};
+
+    for (int i = 0; i < 6; i++)
+        for (int j = 0; j < 6; j++)
+            ASSERT_NEAR(J.at<float>(i, j), expected_jac[i][j], 1e-6);
+
+    cv::Vec6d result = twist.compute(uv, duv, depth, K);
+    float expected_twist[6] = {0.5, 0.5, 1.875, 0.041667, -0.041667, -0.5};
+    for (int i = 0; i < 6; i++)
+        ASSERT_NEAR(result[i], expected_twist[i], 1e-6);
+}
+
 } // namespace
 } // namespace opencv_test

From 5a7967f254e0e3c30cd8fbe9895c62bf6ab8906a Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Fri, 8 Mar 2024 13:27:15 +0100
Subject: [PATCH 06/11] shape asserts

---
 modules/tracking/src/twist.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/modules/tracking/src/twist.cpp b/modules/tracking/src/twist.cpp
index 5494bbdafa6..8ea5595eb88 100644
--- a/modules/tracking/src/twist.cpp
+++ b/modules/tracking/src/twist.cpp
@@ -12,7 +12,8 @@ inline namespace tracking
 void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J)
 {
     CV_Assert(uv.cols == depth.cols);
-    CV_Assert(depth.type() == CV_32F); // Validate depth input type
+    CV_Assert(depth.type() == CV_32F);
+    CV_Assert(K.cols == 3 && K.rows == 3);
 
     J.create(depth.cols * 2, 6, CV_32F);
     J.setTo(0);
@@ -50,6 +51,8 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
 cv::Vec6d Twist::compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat depths,
                          const cv::Mat& K)
 {
+    CV_Assert(uv.cols * 2 == duv.rows);
+
     cv::Mat J;
     interactionMatrix(uv, depths, K, J);
     cv::Mat Jinv;

From bed2b5a1bf3c1672a6c6eed178124af0935f8445 Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Fri, 8 Mar 2024 13:27:36 +0100
Subject: [PATCH 07/11] docs

---
 .../include/opencv2/tracking/twist.hpp        | 28 +++++++++++++++++--
 1 file changed, 26 insertions(+), 2 deletions(-)

diff --git a/modules/tracking/include/opencv2/tracking/twist.hpp b/modules/tracking/include/opencv2/tracking/twist.hpp
index c7682f91597..f59c5004e4e 100644
--- a/modules/tracking/include/opencv2/tracking/twist.hpp
+++ b/modules/tracking/include/opencv2/tracking/twist.hpp
@@ -21,11 +21,35 @@ class CV_EXPORTS Twist
 public:
     Twist() = default;
 
-    // TODO(ernie): docs
+    /**
+     * @brief Compute the camera twist from a set of 2D pixel locations, their
+     * velocities, depth values and intrinsic parameters of the camera. The pixel
+     * velocities are usually obtained from optical flow algorithms, both dense and
+     * sparse flow can be used to compute the flow between images and duv computed by
+     * dividing the flow by the time interval between the images.
+     *
+     * @param uv 2xN matrix of 2D pixel locations
+     * @param duv 2Nx1 matrix of 2D pixel velocities
+     * @param depths 1xN matrix of depth values
+     * @param K 3x3 camera intrinsic matrix
+     *
+     * @return cv::Vec6d 6x1 camera twist
+     */
     cv::Vec6d compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat depths,
                       const cv::Mat& K);
 
-    // TODO(ernie): docs
+    /**
+     * @brief Compute the interaction matrix for a set of 2D pixels. This is usually
+     * used in visual servoing applications to command a robot to move at desired pixel
+     * locations/velocities. By inverting this matrix one can estimate camera spatial
+     * velocity i.e., the twist.
+     *
+     * @param uv 2xN matrix of 2D pixel locations
+     * @param depth 1xN matrix of depth values
+     * @param K 3x3 camera intrinsic matrix
+     * @param J 2Nx6 interaction matrix
+     *
+     */
     void interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J);
 };
 

From 3031e4b9d1a088f4987f589d0331faf94f660edf Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Sun, 10 Mar 2024 12:11:21 +0100
Subject: [PATCH 08/11] fix test issues

---
 .../include/opencv2/tracking/twist.hpp        |  4 ---
 modules/tracking/test/test_twist.cpp          | 25 +++++++++++--------
 2 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/modules/tracking/include/opencv2/tracking/twist.hpp b/modules/tracking/include/opencv2/tracking/twist.hpp
index f59c5004e4e..55ede89f9f5 100644
--- a/modules/tracking/include/opencv2/tracking/twist.hpp
+++ b/modules/tracking/include/opencv2/tracking/twist.hpp
@@ -5,10 +5,6 @@
 
 namespace cv
 {
-class CV_EXPORTS Twist
-{
-};
-
 namespace detail
 {
 inline namespace tracking
diff --git a/modules/tracking/test/test_twist.cpp b/modules/tracking/test/test_twist.cpp
index cac7158f094..dedd7e5104a 100644
--- a/modules/tracking/test/test_twist.cpp
+++ b/modules/tracking/test/test_twist.cpp
@@ -3,6 +3,8 @@
 #include "opencv2/core.hpp"
 #include "opencv2/tracking/twist.hpp"
 
+#define eps 1e-4
+
 namespace opencv_test
 {
 namespace
@@ -38,10 +40,11 @@ TEST_F(TwistTest, TestInteractionMatrix)
     twist.interactionMatrix(uv, depth, K, J);
     ASSERT_EQ(J.cols, 6);
     ASSERT_EQ(J.rows, 2);
-    float expected[2][6] = {{-0.5, 0.0, 0.5, 1.0, -2.0, 1.0}, {0.0, -0.5, 0.5, 2.0, -1.0, -1.0}};
+    float expected[2][6] = {{-0.5f, 0.0f, 0.5f, 1.0f, -2.0f, 1.0f},
+                            {0.0f, -0.5f, 0.5f, 2.0f, -1.0f, -1.0f}};
     for (int i = 0; i < 2; i++)
         for (int j = 0; j < 6; j++)
-            ASSERT_NEAR(J.at<float>(i, j), expected[i][j], 1e-6);
+            ASSERT_NEAR(J.at<float>(i, j), expected[i][j], eps);
 }
 
 TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
@@ -52,7 +55,7 @@ TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
 
     cv::Vec6d result = twist.compute(uv, duv, depth, K);
     for (int i = 0; i < 6; i++)
-        ASSERT_NEAR(result[i], 0.0, 1e-6);
+        ASSERT_NEAR(result[i], 0.0, eps);
 }
 
 TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
@@ -82,19 +85,21 @@ TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
     twist.interactionMatrix(uv, depth, K, J);
     ASSERT_EQ(J.cols, 6);
     ASSERT_EQ(J.rows, 6);
-    float expected_jac[6][6] = {
-        {-1.0, 0.0, 1.0, 1.0, -2.0, 1.0},       {0.0, -1.0, 1.0, 2.0, -1.0, -1.0},
-        {-0.5, 0.0, 1.0, 4.0, -5.0, 2.0},       {0.0, -0.5, 1.0, 5.0, -4.0, -2.0},
-        {-0.333333, 0.0, 1.0, 9.0, -10.0, 3.0}, {0.0, -0.333333, 1.0, 10.0, -9.0, -3.0}};
+    float expected_jac[6][6] = {{-1.0f, 0.0f, 1.0f, 1.0f, -2.0f, 1.0f},
+                                {0.0f, -1.0f, 1.0f, 2.0f, -1.0f, -1.0f},
+                                {-0.5f, 0.0f, 1.0f, 4.0f, -5.0f, 2.0f},
+                                {0.0f, -0.5f, 1.0f, 5.0f, -4.0f, -2.0f},
+                                {-0.333333f, 0.0f, 1.0f, 9.0f, -10.0f, 3.0f},
+                                {0.0f, -0.333333f, 1.0f, 10.0f, -9.0f, -3.0f}};
 
     for (int i = 0; i < 6; i++)
         for (int j = 0; j < 6; j++)
-            ASSERT_NEAR(J.at<float>(i, j), expected_jac[i][j], 1e-6);
+            ASSERT_NEAR(J.at<float>(i, j), expected_jac[i][j], eps);
 
     cv::Vec6d result = twist.compute(uv, duv, depth, K);
-    float expected_twist[6] = {0.5, 0.5, 1.875, 0.041667, -0.041667, -0.5};
+    float expected_twist[6] = {0.5f, 0.5f, 1.875f, 0.041667f, -0.041667f, -0.5f};
     for (int i = 0; i < 6; i++)
-        ASSERT_NEAR(result[i], expected_twist[i], 1e-6);
+        ASSERT_NEAR(result[i], expected_twist[i], eps);
 }
 
 } // namespace

From 3310504b4a58c5ca844a43824ea3c294abe3e194 Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Wed, 13 Mar 2024 10:29:11 +0100
Subject: [PATCH 09/11] refactor class to functions & cv::Mat initialiation

---
 .../include/opencv2/tracking/twist.hpp        | 67 +++++++++----------
 modules/tracking/src/twist.cpp                | 30 ++++++---
 modules/tracking/test/test_twist.cpp          | 51 +++++++++-----
 3 files changed, 85 insertions(+), 63 deletions(-)

diff --git a/modules/tracking/include/opencv2/tracking/twist.hpp b/modules/tracking/include/opencv2/tracking/twist.hpp
index 55ede89f9f5..c6ba60ec73d 100644
--- a/modules/tracking/include/opencv2/tracking/twist.hpp
+++ b/modules/tracking/include/opencv2/tracking/twist.hpp
@@ -12,42 +12,37 @@ inline namespace tracking
 //! @addtogroup tracking_detail
 //! @{
 
-class CV_EXPORTS Twist
-{
-public:
-    Twist() = default;
-
-    /**
-     * @brief Compute the camera twist from a set of 2D pixel locations, their
-     * velocities, depth values and intrinsic parameters of the camera. The pixel
-     * velocities are usually obtained from optical flow algorithms, both dense and
-     * sparse flow can be used to compute the flow between images and duv computed by
-     * dividing the flow by the time interval between the images.
-     *
-     * @param uv 2xN matrix of 2D pixel locations
-     * @param duv 2Nx1 matrix of 2D pixel velocities
-     * @param depths 1xN matrix of depth values
-     * @param K 3x3 camera intrinsic matrix
-     *
-     * @return cv::Vec6d 6x1 camera twist
-     */
-    cv::Vec6d compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat depths,
-                      const cv::Mat& K);
-
-    /**
-     * @brief Compute the interaction matrix for a set of 2D pixels. This is usually
-     * used in visual servoing applications to command a robot to move at desired pixel
-     * locations/velocities. By inverting this matrix one can estimate camera spatial
-     * velocity i.e., the twist.
-     *
-     * @param uv 2xN matrix of 2D pixel locations
-     * @param depth 1xN matrix of depth values
-     * @param K 3x3 camera intrinsic matrix
-     * @param J 2Nx6 interaction matrix
-     *
-     */
-    void interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J);
-};
+/**
+ * @brief Compute the camera twist from a set of 2D pixel locations, their
+ * velocities, depth values and intrinsic parameters of the camera. The pixel
+ * velocities are usually obtained from optical flow algorithms, both dense and
+ * sparse flow can be used to compute the flow between images and duv computed by
+ * dividing the flow by the time interval between the images.
+ *
+ * @param uv 2xN matrix of 2D pixel locations
+ * @param duv 2Nx1 matrix of 2D pixel velocities
+ * @param depths 1xN matrix of depth values
+ * @param K 3x3 camera intrinsic matrix
+ *
+ * @return cv::Vec6d 6x1 camera twist
+ */
+CV_EXPORTS cv::Vec6d computeTwist(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat& depths,
+                                  const cv::Mat& K);
+
+/**
+ * @brief Compute the interaction matrix for a set of 2D pixels. This is usually
+ * used in visual servoing applications to command a robot to move at desired pixel
+ * locations/velocities. By inverting this matrix one can estimate camera spatial
+ * velocity i.e., the twist.
+ *
+ * @param uv 2xN matrix of 2D pixel locations
+ * @param depth 1xN matrix of depth values
+ * @param K 3x3 camera intrinsic matrix
+ * @param J 2Nx6 interaction matrix
+ *
+ */
+CV_EXPORTS void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K,
+                                     cv::Mat& J);
 
 //! @}
 
diff --git a/modules/tracking/src/twist.cpp b/modules/tracking/src/twist.cpp
index 8ea5595eb88..1ff84c42582 100644
--- a/modules/tracking/src/twist.cpp
+++ b/modules/tracking/src/twist.cpp
@@ -9,13 +9,13 @@ namespace detail
 inline namespace tracking
 {
 
-void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J)
+void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K, cv::Mat& J)
 {
-    CV_Assert(uv.cols == depth.cols);
-    CV_Assert(depth.type() == CV_32F);
+    CV_Assert(uv.cols == depths.cols);
+    CV_Assert(depths.type() == CV_32F);
     CV_Assert(K.cols == 3 && K.rows == 3);
 
-    J.create(depth.cols * 2, 6, CV_32F);
+    J.create(depths.cols * 2, 6, CV_32F);
     J.setTo(0);
 
     cv::Mat Kinv;
@@ -25,7 +25,7 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
     cv::Mat Jp(2, 6, CV_32F);
     for (int i = 0; i < uv.cols; i++)
     {
-        const float z = depth.at<float>(i);
+        const float z = depths.at<float>(i);
         // skip points with zero depth
         if (cv::abs(z) < 0.001f)
             continue;
@@ -38,8 +38,18 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
         float y = xy.at<float>(1);
 
         // 2x6 Jacobian for this point
-        Jp = (cv::Mat_<float>(2, 6) << -1 / z, 0.0, x / z, x * y, -(1 + x * x), y, 0.0, -1 / z,
-              y / z, 1 + y * y, -x * y, -x);
+        Jp.at<float>(0, 0) = -1 / z;
+        Jp.at<float>(0, 1) = 0.0;
+        Jp.at<float>(0, 2) = x / z;
+        Jp.at<float>(0, 3) = x * y;
+        Jp.at<float>(0, 4) = -(1 + x * x);
+        Jp.at<float>(0, 5) = y;
+        Jp.at<float>(1, 0) = 0.0;
+        Jp.at<float>(1, 1) = -1 / z;
+        Jp.at<float>(1, 2) = y / z;
+        Jp.at<float>(1, 3) = 1 + y * y;
+        Jp.at<float>(1, 4) = -x * y;
+        Jp.at<float>(1, 5) = -x;
 
         Jp = K(cv::Rect(0, 0, 2, 2)) * Jp;
 
@@ -48,13 +58,13 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
     }
 }
 
-cv::Vec6d Twist::compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat depths,
-                         const cv::Mat& K)
+cv::Vec6d computeTwist(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat& depths,
+                       const cv::Mat& K)
 {
     CV_Assert(uv.cols * 2 == duv.rows);
 
     cv::Mat J;
-    interactionMatrix(uv, depths, K, J);
+    getInteractionMatrix(uv, depths, K, J);
     cv::Mat Jinv;
     cv::invert(J, Jinv, cv::DECOMP_SVD);
     cv::Mat twist = Jinv * duv;
diff --git a/modules/tracking/test/test_twist.cpp b/modules/tracking/test/test_twist.cpp
index dedd7e5104a..250feb458d6 100644
--- a/modules/tracking/test/test_twist.cpp
+++ b/modules/tracking/test/test_twist.cpp
@@ -3,22 +3,24 @@
 #include "opencv2/core.hpp"
 #include "opencv2/tracking/twist.hpp"
 
-#define eps 1e-4
-
 namespace opencv_test
 {
 namespace
 {
 
+using namespace cv::detail::tracking;
+
+float const eps = 1e-4f;
+
 class TwistTest : public ::testing::Test
 {
 protected:
-    cv::detail::tracking::Twist twist;
-    cv::Mat K, J;
+    cv::Mat J, K;
 
     void SetUp() override
     {
-        K = (cv::Mat_<float>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
+        cv::Matx33f K = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
+        this->K = cv::Mat(K);
     }
 };
 
@@ -34,10 +36,13 @@ TEST_F(TwistTest, TestInteractionMatrix)
     // [[-0.5  0.   0.5  1.  -2.   1. ]
     //  [ 0.  -0.5  0.5  2.  -1.  -1. ]]
 
-    cv::Mat uv = (cv::Mat_<float>(2, 1) << 1.0, 1.0);
-    cv::Mat depth = (cv::Mat_<float>(1, 1) << 2.0);
+    // float data[] = {1.5f, 2.0f, 3.0f, 4.2f};
+    // cv::Mat mat = cv::Mat(2, 2, CV_32F, data);
 
-    twist.interactionMatrix(uv, depth, K, J);
+    cv::Mat uv = cv::Mat(2, 1, CV_32F, {1.0f, 1.0f});
+    cv::Mat depth = cv::Mat(1, 1, CV_32F, {2.0f});
+
+    getInteractionMatrix(uv, depth, K, J);
     ASSERT_EQ(J.cols, 6);
     ASSERT_EQ(J.rows, 2);
     float expected[2][6] = {{-0.5f, 0.0f, 0.5f, 1.0f, -2.0f, 1.0f},
@@ -49,11 +54,11 @@ TEST_F(TwistTest, TestInteractionMatrix)
 
 TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
 {
-    cv::Mat uv = (cv::Mat_<float>(2, 2) << 0.0, 0.0, 0.0, 0.0);
-    cv::Mat depth = (cv::Mat_<float>(1, 2) << 1.0, 1.0);
-    cv::Mat duv = (cv::Mat_<float>(4, 1) << 0.0, 0.0, 0.0, 0.0);
+    cv::Mat uv = cv::Mat(2, 2, CV_32F, {1.0f, 0.0f, 3.0f, 0.0f});
+    cv::Mat depths = cv::Mat(1, 2, CV_32F, {1.1f, 1.0f});
+    cv::Mat duv = cv::Mat(4, 1, CV_32F, {0.0f, 0.0f, 0.0f, 0.0f});
 
-    cv::Vec6d result = twist.compute(uv, duv, depth, K);
+    cv::Vec6d result = computeTwist(uv, duv, depths, K);
     for (int i = 0; i < 6; i++)
         ASSERT_NEAR(result[i], 0.0, eps);
 }
@@ -78,11 +83,23 @@ TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
     //  [ -0.333333   0.         1.         9.       -10.         3.      ]
     //  [  0.        -0.333333   1.        10.        -9.        -3.      ]]
 
-    cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
-    cv::Mat depth = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
-    cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);
+    // cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
+    // std::vector<float> depth_data = {1.0f, 2.0f, 3.0f};
+    // cv::Mat depth = cv::Mat(depth_data).reshape(0, 3);
+    // cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);
+
+    // cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
+    // cv::Mat depth = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
+    // cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);
+
+    float uv_data[] = {1.0f, 2.0f, 3.0f, 1.0f, 2.0f, 3.0f};
+    cv::Mat uv = cv::Mat(2, 3, CV_32F, uv_data);
+    float depth_data[] = {1.0f, 2.0f, 3.0f};
+    cv::Mat depth = cv::Mat(1, 3, CV_32F, depth_data);
+    float duv_data[] = {1.0f, 2.0f, 1.0f, 3.0f, 1.0f, 4.0f};
+    cv::Mat duv = cv::Mat(6, 1, CV_32F, duv_data);
 
-    twist.interactionMatrix(uv, depth, K, J);
+    getInteractionMatrix(uv, depth, K, J);
     ASSERT_EQ(J.cols, 6);
     ASSERT_EQ(J.rows, 6);
     float expected_jac[6][6] = {{-1.0f, 0.0f, 1.0f, 1.0f, -2.0f, 1.0f},
@@ -96,7 +113,7 @@ TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
         for (int j = 0; j < 6; j++)
             ASSERT_NEAR(J.at<float>(i, j), expected_jac[i][j], eps);
 
-    cv::Vec6d result = twist.compute(uv, duv, depth, K);
+    cv::Vec6d result = computeTwist(uv, duv, depth, K);
     float expected_twist[6] = {0.5f, 0.5f, 1.875f, 0.041667f, -0.041667f, -0.5f};
     for (int i = 0; i < 6; i++)
         ASSERT_NEAR(result[i], expected_twist[i], eps);

From da83de9183db548365152cc9796091299b489538 Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Fri, 15 Mar 2024 11:15:54 +0100
Subject: [PATCH 10/11] delete some dead comments in test code

---
 modules/tracking/test/test_twist.cpp | 12 ------------
 1 file changed, 12 deletions(-)

diff --git a/modules/tracking/test/test_twist.cpp b/modules/tracking/test/test_twist.cpp
index 250feb458d6..3911f28aea8 100644
--- a/modules/tracking/test/test_twist.cpp
+++ b/modules/tracking/test/test_twist.cpp
@@ -36,9 +36,6 @@ TEST_F(TwistTest, TestInteractionMatrix)
     // [[-0.5  0.   0.5  1.  -2.   1. ]
     //  [ 0.  -0.5  0.5  2.  -1.  -1. ]]
 
-    // float data[] = {1.5f, 2.0f, 3.0f, 4.2f};
-    // cv::Mat mat = cv::Mat(2, 2, CV_32F, data);
-
     cv::Mat uv = cv::Mat(2, 1, CV_32F, {1.0f, 1.0f});
     cv::Mat depth = cv::Mat(1, 1, CV_32F, {2.0f});
 
@@ -83,15 +80,6 @@ TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
     //  [ -0.333333   0.         1.         9.       -10.         3.      ]
     //  [  0.        -0.333333   1.        10.        -9.        -3.      ]]
 
-    // cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
-    // std::vector<float> depth_data = {1.0f, 2.0f, 3.0f};
-    // cv::Mat depth = cv::Mat(depth_data).reshape(0, 3);
-    // cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);
-
-    // cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
-    // cv::Mat depth = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
-    // cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);
-
     float uv_data[] = {1.0f, 2.0f, 3.0f, 1.0f, 2.0f, 3.0f};
     cv::Mat uv = cv::Mat(2, 3, CV_32F, uv_data);
     float depth_data[] = {1.0f, 2.0f, 3.0f};

From 9c5b093be205473d6bde8f3767f0cd910e91c7e8 Mon Sep 17 00:00:00 2001
From: ernie <simutisernestas@gmail.com>
Date: Sun, 17 Mar 2024 18:28:37 +0100
Subject: [PATCH 11/11] fix docs param name

---
 modules/tracking/include/opencv2/tracking/twist.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/modules/tracking/include/opencv2/tracking/twist.hpp b/modules/tracking/include/opencv2/tracking/twist.hpp
index c6ba60ec73d..8d998beda33 100644
--- a/modules/tracking/include/opencv2/tracking/twist.hpp
+++ b/modules/tracking/include/opencv2/tracking/twist.hpp
@@ -36,7 +36,7 @@ CV_EXPORTS cv::Vec6d computeTwist(const cv::Mat& uv, const cv::Mat& duv, const c
  * velocity i.e., the twist.
  *
  * @param uv 2xN matrix of 2D pixel locations
- * @param depth 1xN matrix of depth values
+ * @param depths 1xN matrix of depth values
  * @param K 3x3 camera intrinsic matrix
  * @param J 2Nx6 interaction matrix
  *