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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
|
#include "perf_precomp.hpp"
namespace opencv_test
{
using namespace perf;
CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS, SOLVEPNP_UPNP)
typedef tuple<int, pnpAlgo> PointsNum_Algo_t;
typedef perf::TestBaseWithParam<PointsNum_Algo_t> PointsNum_Algo;
typedef perf::TestBaseWithParam<int> PointsNum;
PERF_TEST_P(PointsNum_Algo, solvePnP,
testing::Combine( //When non planar, DLT needs at least 6 points for SOLVEPNP_ITERATIVE flag
testing::Values(6, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS)
)
)
{
int pointsNum = get<0>(GetParam());
pnpAlgo algo = get<1>(GetParam());
vector<Point2f> points2d(pointsNum);
vector<Point3f> points3d(pointsNum);
Mat rvec = Mat::zeros(3, 1, CV_32FC1);
Mat tvec = Mat::zeros(3, 1, CV_32FC1);
Mat distortion = Mat::zeros(5, 1, CV_32FC1);
Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
intrinsics.at<float> (0, 0) = 400.0;
intrinsics.at<float> (1, 1) = 400.0;
intrinsics.at<float> (0, 2) = 640 / 2;
intrinsics.at<float> (1, 2) = 480 / 2;
warmup(points3d, WARMUP_RNG);
warmup(rvec, WARMUP_RNG);
warmup(tvec, WARMUP_RNG);
projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
//add noise
Mat noise(1, (int)points2d.size(), CV_32FC2);
randu(noise, 0, 0.01);
cv::add(points2d, noise, points2d);
declare.in(points3d, points2d);
declare.time(100);
TEST_CYCLE_N(1000)
{
cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
}
SANITY_CHECK(rvec, 1e-4);
SANITY_CHECK(tvec, 1e-4);
}
PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
testing::Combine(
testing::Values(5),
testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
)
)
{
int pointsNum = get<0>(GetParam());
pnpAlgo algo = get<1>(GetParam());
if( algo == SOLVEPNP_P3P )
pointsNum = 4;
vector<Point2f> points2d(pointsNum);
vector<Point3f> points3d(pointsNum);
Mat rvec = Mat::zeros(3, 1, CV_32FC1);
Mat tvec = Mat::zeros(3, 1, CV_32FC1);
Mat distortion = Mat::zeros(5, 1, CV_32FC1);
Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
intrinsics.at<float> (0, 0) = 400.0f;
intrinsics.at<float> (1, 1) = 400.0f;
intrinsics.at<float> (0, 2) = 640 / 2;
intrinsics.at<float> (1, 2) = 480 / 2;
warmup(points3d, WARMUP_RNG);
warmup(rvec, WARMUP_RNG);
warmup(tvec, WARMUP_RNG);
// normalize Rodrigues vector
Mat rvec_tmp = Mat::eye(3, 3, CV_32F);
cv::Rodrigues(rvec, rvec_tmp);
cv::Rodrigues(rvec_tmp, rvec);
cv::projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
//add noise
Mat noise(1, (int)points2d.size(), CV_32FC2);
randu(noise, -0.001, 0.001);
cv::add(points2d, noise, points2d);
declare.in(points3d, points2d);
declare.time(100);
TEST_CYCLE_N(1000)
{
cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
}
SANITY_CHECK(rvec, 1e-1);
SANITY_CHECK(tvec, 1e-2);
}
PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(5, 3*9, 7*13))
{
int count = GetParam();
Mat object(1, count, CV_32FC3);
randu(object, -100, 100);
Mat camera_mat(3, 3, CV_32FC1);
randu(camera_mat, 0.5, 1);
camera_mat.at<float>(0, 1) = 0.f;
camera_mat.at<float>(1, 0) = 0.f;
camera_mat.at<float>(2, 0) = 0.f;
camera_mat.at<float>(2, 1) = 0.f;
Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0));
vector<cv::Point2f> image_vec;
Mat rvec_gold(1, 3, CV_32FC1);
randu(rvec_gold, 0, 1);
Mat tvec_gold(1, 3, CV_32FC1);
randu(tvec_gold, 0, 1);
projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec);
Mat image(1, count, CV_32FC2, &image_vec[0]);
Mat rvec;
Mat tvec;
TEST_CYCLE()
{
cv::solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
}
SANITY_CHECK(rvec, 1e-6);
SANITY_CHECK(tvec, 1e-6);
}
} // namespace
|
