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