Skip to content

Commit 1400863

Browse files
asmorkalovasmorkalov
committed
Merge branch 4.x
2 parents d72bcfd + ac994ed commit 1400863

File tree

4 files changed

+303
-63
lines changed

4 files changed

+303
-63
lines changed

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

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
#ifndef OPENCV_TWIST_HPP
2+
#define OPENCV_TWIST_HPP
3+
4+
#include "opencv2/core.hpp"
5+
6+
namespace cv
7+
{
8+
namespace detail
9+
{
10+
inline namespace tracking
11+
{
12+
//! @addtogroup tracking_detail
13+
//! @{
14+
15+
/**
16+
* @brief Compute the camera twist from a set of 2D pixel locations, their
17+
* velocities, depth values and intrinsic parameters of the camera. The pixel
18+
* velocities are usually obtained from optical flow algorithms, both dense and
19+
* sparse flow can be used to compute the flow between images and duv computed by
20+
* dividing the flow by the time interval between the images.
21+
*
22+
* @param uv 2xN matrix of 2D pixel locations
23+
* @param duv 2Nx1 matrix of 2D pixel velocities
24+
* @param depths 1xN matrix of depth values
25+
* @param K 3x3 camera intrinsic matrix
26+
*
27+
* @return cv::Vec6d 6x1 camera twist
28+
*/
29+
CV_EXPORTS cv::Vec6d computeTwist(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat& depths,
30+
const cv::Mat& K);
31+
32+
/**
33+
* @brief Compute the interaction matrix for a set of 2D pixels. This is usually
34+
* used in visual servoing applications to command a robot to move at desired pixel
35+
* locations/velocities. By inverting this matrix one can estimate camera spatial
36+
* velocity i.e., the twist.
37+
*
38+
* @param uv 2xN matrix of 2D pixel locations
39+
* @param depths 1xN matrix of depth values
40+
* @param K 3x3 camera intrinsic matrix
41+
* @param J 2Nx6 interaction matrix
42+
*
43+
*/
44+
CV_EXPORTS void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K,
45+
cv::Mat& J);
46+
47+
//! @}
48+
49+
} // namespace tracking
50+
} // namespace detail
51+
} // namespace cv
52+
53+
#endif

‎modules/tracking/src/twist.cpp

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
2+
#include "precomp.hpp"
3+
#include "opencv2/tracking/twist.hpp"
4+
5+
namespace cv
6+
{
7+
namespace detail
8+
{
9+
inline namespace tracking
10+
{
11+
12+
void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K, cv::Mat& J)
13+
{
14+
CV_Assert(uv.cols == depths.cols);
15+
CV_Assert(depths.type() == CV_32F);
16+
CV_Assert(K.cols == 3 && K.rows == 3);
17+
18+
J.create(depths.cols * 2, 6, CV_32F);
19+
J.setTo(0);
20+
21+
cv::Mat Kinv;
22+
cv::invert(K, Kinv);
23+
24+
cv::Mat xy(3, 1, CV_32F);
25+
cv::Mat Jp(2, 6, CV_32F);
26+
for (int i = 0; i < uv.cols; i++)
27+
{
28+
const float z = depths.at<float>(i);
29+
// skip points with zero depth
30+
if (cv::abs(z) < 0.001f)
31+
continue;
32+
33+
const cv::Point3f p(uv.at<float>(0, i), uv.at<float>(1, i), 1.0);
34+
35+
// convert to normalized image-plane coordinates
36+
xy = Kinv * cv::Mat(p);
37+
float x = xy.at<float>(0);
38+
float y = xy.at<float>(1);
39+
40+
// 2x6 Jacobian for this point
41+
Jp.at<float>(0, 0) = -1 / z;
42+
Jp.at<float>(0, 1) = 0.0;
43+
Jp.at<float>(0, 2) = x / z;
44+
Jp.at<float>(0, 3) = x * y;
45+
Jp.at<float>(0, 4) = -(1 + x * x);
46+
Jp.at<float>(0, 5) = y;
47+
Jp.at<float>(1, 0) = 0.0;
48+
Jp.at<float>(1, 1) = -1 / z;
49+
Jp.at<float>(1, 2) = y / z;
50+
Jp.at<float>(1, 3) = 1 + y * y;
51+
Jp.at<float>(1, 4) = -x * y;
52+
Jp.at<float>(1, 5) = -x;
53+
54+
Jp = K(cv::Rect(0, 0, 2, 2)) * Jp;
55+
56+
// push into Jacobian
57+
Jp.copyTo(J(cv::Rect(0, 2 * i, 6, 2)));
58+
}
59+
}
60+
61+
cv::Vec6d computeTwist(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat& depths,
62+
const cv::Mat& K)
63+
{
64+
CV_Assert(uv.cols * 2 == duv.rows);
65+
66+
cv::Mat J;
67+
getInteractionMatrix(uv, depths, K, J);
68+
cv::Mat Jinv;
69+
cv::invert(J, Jinv, cv::DECOMP_SVD);
70+
cv::Mat twist = Jinv * duv;
71+
return twist;
72+
}
73+
74+
} // namespace tracking
75+
} // namespace detail
76+
} // namespace cv

‎modules/tracking/test/test_twist.cpp

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
#include "test_precomp.hpp"
2+
3+
#include "opencv2/core.hpp"
4+
#include "opencv2/tracking/twist.hpp"
5+
6+
namespace opencv_test
7+
{
8+
namespace
9+
{
10+
11+
using namespace cv::detail::tracking;
12+
13+
float const eps = 1e-4f;
14+
15+
class TwistTest : public ::testing::Test
16+
{
17+
protected:
18+
cv::Mat J, K;
19+
20+
void SetUp() override
21+
{
22+
cv::Matx33f K = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
23+
this->K = cv::Mat(K);
24+
}
25+
};
26+
27+
TEST_F(TwistTest, TestInteractionMatrix)
28+
{
29+
// import machinevisiontoolbox as mv
30+
// cam = mv.CentralCamera()
31+
// print(cam.K)
32+
// print(cam.visjac_p([1, 1], 2.0))
33+
// [[1. 0. 0.]
34+
// [0. 1. 0.]
35+
// [0. 0. 1.]]
36+
// [[-0.5 0. 0.5 1. -2. 1. ]
37+
// [ 0. -0.5 0.5 2. -1. -1. ]]
38+
39+
cv::Mat uv = cv::Mat(2, 1, CV_32F, {1.0f, 1.0f});
40+
cv::Mat depth = cv::Mat(1, 1, CV_32F, {2.0f});
41+
42+
getInteractionMatrix(uv, depth, K, J);
43+
ASSERT_EQ(J.cols, 6);
44+
ASSERT_EQ(J.rows, 2);
45+
float expected[2][6] = {{-0.5f, 0.0f, 0.5f, 1.0f, -2.0f, 1.0f},
46+
{0.0f, -0.5f, 0.5f, 2.0f, -1.0f, -1.0f}};
47+
for (int i = 0; i < 2; i++)
48+
for (int j = 0; j < 6; j++)
49+
ASSERT_NEAR(J.at<float>(i, j), expected[i][j], eps);
50+
}
51+
52+
TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
53+
{
54+
cv::Mat uv = cv::Mat(2, 2, CV_32F, {1.0f, 0.0f, 3.0f, 0.0f});
55+
cv::Mat depths = cv::Mat(1, 2, CV_32F, {1.1f, 1.0f});
56+
cv::Mat duv = cv::Mat(4, 1, CV_32F, {0.0f, 0.0f, 0.0f, 0.0f});
57+
58+
cv::Vec6d result = computeTwist(uv, duv, depths, K);
59+
for (int i = 0; i < 6; i++)
60+
ASSERT_NEAR(result[i], 0.0, eps);
61+
}
62+
63+
TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
64+
{
65+
// import machinevisiontoolbox as mv
66+
// cam = mv.CentralCamera()
67+
// pixels = np.array([[1, 2, 3],
68+
// [1, 2, 3]], dtype=float)
69+
// depths = np.array([1.0, 2.0, 3.0])
70+
// Jac = cam.visjac_p(pixels, depths)
71+
// duv = np.array([1, 2, 1, 3, 1, 4])
72+
// twist = np.linalg.lstsq(Jac, duv, rcond=None)[0]
73+
// print(twist)
74+
// print(Jac)
75+
// [ 0.5 0.5 1.875 0.041667 -0.041667 -0.5 ]
76+
// [[ -1. 0. 1. 1. -2. 1. ]
77+
// [ 0. -1. 1. 2. -1. -1. ]
78+
// [ -0.5 0. 1. 4. -5. 2. ]
79+
// [ 0. -0.5 1. 5. -4. -2. ]
80+
// [ -0.333333 0. 1. 9. -10. 3. ]
81+
// [ 0. -0.333333 1. 10. -9. -3. ]]
82+
83+
float uv_data[] = {1.0f, 2.0f, 3.0f, 1.0f, 2.0f, 3.0f};
84+
cv::Mat uv = cv::Mat(2, 3, CV_32F, uv_data);
85+
float depth_data[] = {1.0f, 2.0f, 3.0f};
86+
cv::Mat depth = cv::Mat(1, 3, CV_32F, depth_data);
87+
float duv_data[] = {1.0f, 2.0f, 1.0f, 3.0f, 1.0f, 4.0f};
88+
cv::Mat duv = cv::Mat(6, 1, CV_32F, duv_data);
89+
90+
getInteractionMatrix(uv, depth, K, J);
91+
ASSERT_EQ(J.cols, 6);
92+
ASSERT_EQ(J.rows, 6);
93+
float expected_jac[6][6] = {{-1.0f, 0.0f, 1.0f, 1.0f, -2.0f, 1.0f},
94+
{0.0f, -1.0f, 1.0f, 2.0f, -1.0f, -1.0f},
95+
{-0.5f, 0.0f, 1.0f, 4.0f, -5.0f, 2.0f},
96+
{0.0f, -0.5f, 1.0f, 5.0f, -4.0f, -2.0f},
97+
{-0.333333f, 0.0f, 1.0f, 9.0f, -10.0f, 3.0f},
98+
{0.0f, -0.333333f, 1.0f, 10.0f, -9.0f, -3.0f}};
99+
100+
for (int i = 0; i < 6; i++)
101+
for (int j = 0; j < 6; j++)
102+
ASSERT_NEAR(J.at<float>(i, j), expected_jac[i][j], eps);
103+
104+
cv::Vec6d result = computeTwist(uv, duv, depth, K);
105+
float expected_twist[6] = {0.5f, 0.5f, 1.875f, 0.041667f, -0.041667f, -0.5f};
106+
for (int i = 0; i < 6; i++)
107+
ASSERT_NEAR(result[i], expected_twist[i], eps);
108+
}
109+
110+
} // namespace
111+
} // namespace opencv_test

0 commit comments

Comments
 (0)