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