1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
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});
computeInteractionMatrix(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);
computeInteractionMatrix(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
|
