Merge pull request #3641 from simutisernestas:4.x

Camera spatial velocity computation through interaction matrix #3641

### Feature

The code computed camera spatial velocity given two images, pixels depth, camera matrix and the timestamp between the images. This is enabled by, so called, interaction matrix (usually utilized in visual servoing applications) relating pixel plane velocities to the camera spatial velocity in 3D i.e., twist - velocity and angular rate of the camera. The inverse problem can be solved by sampling pixel & their velocities to solve least-squares for twist. The relationship can be seen below in the picture.

![image](https://github.com/opencv/opencv_contrib/assets/35775651/b83179ba-9d5a-4324-863a-4ad6e158564a)

The code does not include a proper example and is not tested but if there is interest I could contribute more appealing example and use case for camera velocity computation. However, I attach below a dummy example with random data simply to make sure that it's running as is. I have used this before in aiding UAV localization and thought someone else might benefit from it being integrated into `opencv`. 

```c++
#include "opencv2/rgbd/twist.hpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"

int main()
{
    using namespace cv::rgbd;
    Twist t;

    // create two random image
    cv::Mat im0(480, 640, CV_8UC1);
    cv::Mat im1(480, 640, CV_8UC1);
    cv::Mat depths0(480, 640, CV_32F);
    cv::Mat K = cv::Mat::eye(3, 3, CV_32F);

    cv::theRNG().state = time(0);
    cv::randn(im0, 0, 255);
    cv::randn(im1, 0, 255);
    cv::randn(depths0, 0, 100);

    cv::Vec6d twist = t.compute(im0, im1, depths0, K, 0.1);

    return 0;
}

```

References
1. Chaumette, François, and Seth Hutchinson. "Visual servo control. I. Basic approaches." IEEE Robotics & Automation Magazine 13.4 (2006): 82-90.
2. https://robotacademy.net.au/lesson/image-based-visual-servoing/

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake

Author: simutisernestas

modules/tracking/include/opencv2/tracking/twist.hpp

@@ -0,0 +1,53 @@
+#ifndef OPENCV_TWIST_HPP
+#define OPENCV_TWIST_HPP
+
+#include "opencv2/core.hpp"
+
+namespace cv
+{
+namespace detail
+{
+inline namespace tracking
+{
+//! @addtogroup tracking_detail
+//! @{
+
+/**
+ * @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 depths 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);
+
+//! @}
+
+} // namespace tracking
+} // namespace detail
+} // namespace cv
+
+#endif

modules/tracking/src/twist.cpp

@@ -0,0 +1,76 @@
+
+#include "precomp.hpp"
+#include "opencv2/tracking/twist.hpp"
+
+namespace cv
+{
+namespace detail
+{
+inline namespace tracking
+{
+
+void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K, cv::Mat& J)
+{
+    CV_Assert(uv.cols == depths.cols);
+    CV_Assert(depths.type() == CV_32F);
+    CV_Assert(K.cols == 3 && K.rows == 3);
+
+    J.create(depths.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 = depths.at<float>(i);
+        // skip points with zero depth
+        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.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;
+
+        // push into Jacobian
+        Jp.copyTo(J(cv::Rect(0, 2 * i, 6, 2)));
+    }
+}
+
+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;
+    getInteractionMatrix(uv, depths, K, J);
+    cv::Mat Jinv;
+    cv::invert(J, Jinv, cv::DECOMP_SVD);
+    cv::Mat twist = Jinv * duv;
+    return twist;
+}
+
+} // namespace tracking
+} // namespace detail
+} // namespace cv

modules/tracking/test/test_twist.cpp

@@ -0,0 +1,111 @@
+#include "test_precomp.hpp"
+
+#include "opencv2/core.hpp"
+#include "opencv2/tracking/twist.hpp"
+
+namespace opencv_test
+{
+namespace
+{
+
+using namespace cv::detail::tracking;
+
+float const eps = 1e-4f;
+
+class TwistTest : public ::testing::Test
+{
+protected:
+    cv::Mat J, K;
+
+    void SetUp() override
+    {
+        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);
+    }
+};
+
+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(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},
+                            {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], eps);
+}
+
+TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
+{
+    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 = computeTwist(uv, duv, depths, K);
+    for (int i = 0; i < 6; i++)
+        ASSERT_NEAR(result[i], 0.0, eps);
+}
+
+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.      ]]
+
+    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);
+
+    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},
+                                {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], eps);
+
+    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);
+}
+
+} // namespace
+} // namespace opencv_test