5.x merge 4.x

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