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
#include "perf_precomp.hpp"
#ifdef HAVE_TBB
#include "tbb/task_scheduler_init.h"
#endif
using namespace std;
using namespace cv;
using namespace perf;
using std::tr1::make_tuple;
using std::tr1::get;
CV_ENUM(pnpAlgo, ITERATIVE, EPNP /*, P3P*/)
typedef std::tr1::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(
testing::Values(/*4,*/ 3*9, 7*13), //TODO: find why results on 4 points are too unstable
testing::Values((int)ITERATIVE, (int)EPNP)
)
)
{
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);
add(points2d, noise, points2d);
declare.in(points3d, points2d);
TEST_CYCLE_N(1000)
{
solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
}
SANITY_CHECK(rvec, 1e-6);
SANITY_CHECK(tvec, 1e-3);
}
PERF_TEST(PointsNum_Algo, solveP3P)
{
int 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.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);
add(points2d, noise, points2d);
declare.in(points3d, points2d);
declare.time(100);
TEST_CYCLE_N(1000)
{
solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, P3P);
}
SANITY_CHECK(rvec, 1e-6);
SANITY_CHECK(tvec, 1e-6);
}
PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(4, 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;
#ifdef HAVE_TBB
// limit concurrency to get deterministic result
tbb::task_scheduler_init one_thread(1);
#endif
TEST_CYCLE()
{
solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
}
SANITY_CHECK(rvec, 1e-6);
SANITY_CHECK(tvec, 1e-6);
}