1 #include "perf_precomp.hpp"
2 
3 namespace opencv_test
4 {
5 using namespace perf;
6 
7 CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS, SOLVEPNP_UPNP)
8 
9 typedef tuple<int, pnpAlgo> PointsNum_Algo_t;
10 typedef perf::TestBaseWithParam<PointsNum_Algo_t> PointsNum_Algo;
11 
12 typedef perf::TestBaseWithParam<int> PointsNum;
13 
14 PERF_TEST_P(PointsNum_Algo, solvePnP,
15             testing::Combine( //When non planar, DLT needs at least 6 points for SOLVEPNP_ITERATIVE flag
16                 testing::Values(6, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
17                 testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS)
18                 )
19             )
20 {
21     int pointsNum = get<0>(GetParam());
22     pnpAlgo algo = get<1>(GetParam());
23 
24     vector<Point2f> points2d(pointsNum);
25     vector<Point3f> points3d(pointsNum);
26     Mat rvec = Mat::zeros(3, 1, CV_32FC1);
27     Mat tvec = Mat::zeros(3, 1, CV_32FC1);
28 
29     Mat distortion = Mat::zeros(5, 1, CV_32FC1);
30     Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
31     intrinsics.at<float> (0, 0) = 400.0;
32     intrinsics.at<float> (1, 1) = 400.0;
33     intrinsics.at<float> (0, 2) = 640 / 2;
34     intrinsics.at<float> (1, 2) = 480 / 2;
35 
36     warmup(points3d, WARMUP_RNG);
37     warmup(rvec, WARMUP_RNG);
38     warmup(tvec, WARMUP_RNG);
39 
40     projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
41 
42     //add noise
43     Mat noise(1, (int)points2d.size(), CV_32FC2);
44     randu(noise, 0, 0.01);
45     cv::add(points2d, noise, points2d);
46 
47     declare.in(points3d, points2d);
48     declare.time(100);
49 
50     TEST_CYCLE_N(1000)
51     {
52         cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
53     }
54 
55     SANITY_CHECK(rvec, 1e-4);
56     SANITY_CHECK(tvec, 1e-4);
57 }
58 
59 PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
60             testing::Combine(
61                 testing::Values(5),
62                 testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
63                 )
64             )
65 {
66     int pointsNum = get<0>(GetParam());
67     pnpAlgo algo = get<1>(GetParam());
68     if( algo == SOLVEPNP_P3P )
69         pointsNum = 4;
70 
71     vector<Point2f> points2d(pointsNum);
72     vector<Point3f> points3d(pointsNum);
73     Mat rvec = Mat::zeros(3, 1, CV_32FC1);
74     Mat tvec = Mat::zeros(3, 1, CV_32FC1);
75 
76     Mat distortion = Mat::zeros(5, 1, CV_32FC1);
77     Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
78     intrinsics.at<float> (0, 0) = 400.0f;
79     intrinsics.at<float> (1, 1) = 400.0f;
80     intrinsics.at<float> (0, 2) = 640 / 2;
81     intrinsics.at<float> (1, 2) = 480 / 2;
82 
83     warmup(points3d, WARMUP_RNG);
84     warmup(rvec, WARMUP_RNG);
85     warmup(tvec, WARMUP_RNG);
86 
87     // normalize Rodrigues vector
88     Mat rvec_tmp = Mat::eye(3, 3, CV_32F);
89     cv::Rodrigues(rvec, rvec_tmp);
90     cv::Rodrigues(rvec_tmp, rvec);
91 
92     cv::projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
93 
94     //add noise
95     Mat noise(1, (int)points2d.size(), CV_32FC2);
96     randu(noise, -0.001, 0.001);
97     cv::add(points2d, noise, points2d);
98 
99     declare.in(points3d, points2d);
100     declare.time(100);
101 
102     TEST_CYCLE_N(1000)
103     {
104         cv::solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
105     }
106 
107     SANITY_CHECK(rvec, 1e-1);
108     SANITY_CHECK(tvec, 1e-2);
109 }
110 
111 PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(5, 3*9, 7*13))
112 {
113     int count = GetParam();
114 
115     Mat object(1, count, CV_32FC3);
116     randu(object, -100, 100);
117 
118     Mat camera_mat(3, 3, CV_32FC1);
119     randu(camera_mat, 0.5, 1);
120     camera_mat.at<float>(0, 1) = 0.f;
121     camera_mat.at<float>(1, 0) = 0.f;
122     camera_mat.at<float>(2, 0) = 0.f;
123     camera_mat.at<float>(2, 1) = 0.f;
124 
125     Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0));
126 
127     vector<cv::Point2f> image_vec;
128 
129     Mat rvec_gold(1, 3, CV_32FC1);
130     randu(rvec_gold, 0, 1);
131 
132     Mat tvec_gold(1, 3, CV_32FC1);
133     randu(tvec_gold, 0, 1);
134     projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec);
135 
136     Mat image(1, count, CV_32FC2, &image_vec[0]);
137 
138     Mat rvec;
139     Mat tvec;
140 
TEST_CYCLE()141     TEST_CYCLE()
142     {
143         cv::solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
144     }
145 
146     SANITY_CHECK(rvec, 1e-6);
147     SANITY_CHECK(tvec, 1e-6);
148 }
149 
150 } // namespace
151