2011-09-13 09:54:19 +02:00
|
|
|
#include "perf_precomp.hpp"
|
2013-04-01 15:29:10 +02:00
|
|
|
|
|
|
|
#ifdef HAVE_TBB
|
|
|
|
#include "tbb/task_scheduler_init.h"
|
|
|
|
#endif
|
2011-09-13 09:54:19 +02:00
|
|
|
|
|
|
|
using namespace std;
|
|
|
|
using namespace cv;
|
|
|
|
using namespace perf;
|
2011-12-29 17:46:16 +01:00
|
|
|
using std::tr1::make_tuple;
|
|
|
|
using std::tr1::get;
|
2011-09-13 09:54:19 +02:00
|
|
|
|
2014-10-02 16:45:59 +02:00
|
|
|
CV_ENUM(pnpAlgo, SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_DLS, SOLVEPNP_UPNP)
|
2011-09-13 09:54:19 +02:00
|
|
|
|
2011-12-29 17:46:16 +01:00
|
|
|
typedef std::tr1::tuple<int, pnpAlgo> PointsNum_Algo_t;
|
|
|
|
typedef perf::TestBaseWithParam<PointsNum_Algo_t> PointsNum_Algo;
|
2011-09-13 09:54:19 +02:00
|
|
|
|
2012-08-10 14:39:36 +02:00
|
|
|
typedef perf::TestBaseWithParam<int> PointsNum;
|
|
|
|
|
2011-12-29 17:46:16 +01:00
|
|
|
PERF_TEST_P(PointsNum_Algo, solvePnP,
|
|
|
|
testing::Combine(
|
2015-03-13 16:04:23 +01:00
|
|
|
testing::Values(5, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
|
2015-03-16 14:56:26 +01:00
|
|
|
testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS)
|
2011-12-29 17:46:16 +01:00
|
|
|
)
|
|
|
|
)
|
2011-09-13 09:54:19 +02:00
|
|
|
{
|
2011-12-29 17:46:16 +01:00
|
|
|
int pointsNum = get<0>(GetParam());
|
|
|
|
pnpAlgo algo = get<1>(GetParam());
|
2011-09-13 09:54:19 +02:00
|
|
|
|
|
|
|
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
|
2012-03-16 22:21:04 +01:00
|
|
|
Mat noise(1, (int)points2d.size(), CV_32FC2);
|
2011-09-13 09:54:19 +02:00
|
|
|
randu(noise, 0, 0.01);
|
|
|
|
add(points2d, noise, points2d);
|
|
|
|
|
|
|
|
declare.in(points3d, points2d);
|
2014-08-12 18:29:43 +02:00
|
|
|
declare.time(100);
|
2011-09-13 09:54:19 +02:00
|
|
|
|
2012-10-09 16:34:55 +02:00
|
|
|
TEST_CYCLE_N(1000)
|
|
|
|
{
|
|
|
|
solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
|
|
|
|
}
|
2011-12-29 17:46:16 +01:00
|
|
|
|
2014-10-06 17:22:43 +02:00
|
|
|
SANITY_CHECK(rvec, 1e-6);
|
|
|
|
SANITY_CHECK(tvec, 1e-6);
|
2011-12-29 17:46:16 +01:00
|
|
|
}
|
|
|
|
|
2014-08-18 21:46:16 +02:00
|
|
|
PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
|
|
|
|
testing::Combine(
|
2015-03-16 14:56:26 +01:00
|
|
|
testing::Values(5),
|
|
|
|
testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
|
2014-08-18 21:46:16 +02:00
|
|
|
)
|
|
|
|
)
|
2011-12-29 17:46:16 +01:00
|
|
|
{
|
2014-08-18 21:46:16 +02:00
|
|
|
int pointsNum = get<0>(GetParam());
|
|
|
|
pnpAlgo algo = get<1>(GetParam());
|
2015-03-16 14:56:26 +01:00
|
|
|
if( algo == SOLVEPNP_P3P )
|
|
|
|
pointsNum = 4;
|
2011-12-29 17:46:16 +01:00
|
|
|
|
|
|
|
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);
|
2014-10-11 20:57:22 +02:00
|
|
|
intrinsics.at<float> (0, 0) = 400.0f;
|
|
|
|
intrinsics.at<float> (1, 1) = 400.0f;
|
2011-12-29 17:46:16 +01:00
|
|
|
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);
|
|
|
|
|
2015-07-20 16:05:25 +02:00
|
|
|
// normalize Rodrigues vector
|
|
|
|
Mat rvec_tmp = Mat::eye(3, 3, CV_32F);
|
|
|
|
Rodrigues(rvec, rvec_tmp);
|
|
|
|
Rodrigues(rvec_tmp, rvec);
|
|
|
|
|
2011-12-29 17:46:16 +01:00
|
|
|
projectPoints(points3d, rvec, tvec, intrinsics, distortion, points2d);
|
|
|
|
|
|
|
|
//add noise
|
2012-03-16 22:21:04 +01:00
|
|
|
Mat noise(1, (int)points2d.size(), CV_32FC2);
|
2015-03-16 10:46:24 +01:00
|
|
|
randu(noise, -0.001, 0.001);
|
2011-12-29 17:46:16 +01:00
|
|
|
add(points2d, noise, points2d);
|
|
|
|
|
|
|
|
declare.in(points3d, points2d);
|
2012-11-28 14:36:19 +01:00
|
|
|
declare.time(100);
|
2011-12-29 17:46:16 +01:00
|
|
|
|
2012-10-09 16:34:55 +02:00
|
|
|
TEST_CYCLE_N(1000)
|
|
|
|
{
|
2014-08-18 21:46:16 +02:00
|
|
|
solvePnP(points3d, points2d, intrinsics, distortion, rvec, tvec, false, algo);
|
2012-10-09 16:34:55 +02:00
|
|
|
}
|
2011-09-13 09:54:19 +02:00
|
|
|
|
2014-10-06 18:18:03 +02:00
|
|
|
SANITY_CHECK(rvec, 1e-1);
|
2014-08-19 00:22:08 +02:00
|
|
|
SANITY_CHECK(tvec, 1e-2);
|
2011-09-13 09:54:19 +02:00
|
|
|
}
|
2012-08-10 14:39:36 +02:00
|
|
|
|
2015-03-13 16:04:23 +01:00
|
|
|
PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(5, 3*9, 7*13))
|
2012-08-10 14:39:36 +02:00
|
|
|
{
|
|
|
|
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;
|
2014-08-11 17:09:26 +02:00
|
|
|
|
2012-08-10 14:39:36 +02:00
|
|
|
Mat rvec_gold(1, 3, CV_32FC1);
|
|
|
|
randu(rvec_gold, 0, 1);
|
2014-08-11 17:09:26 +02:00
|
|
|
|
2012-08-10 14:39:36 +02:00
|
|
|
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;
|
|
|
|
|
2012-10-09 16:34:55 +02:00
|
|
|
#ifdef HAVE_TBB
|
2014-03-03 16:37:47 +01:00
|
|
|
// limit concurrency to get deterministic result
|
2013-08-15 17:54:00 +02:00
|
|
|
tbb::task_scheduler_init one_thread(1);
|
2012-10-09 16:34:55 +02:00
|
|
|
#endif
|
2012-08-10 14:39:36 +02:00
|
|
|
|
|
|
|
TEST_CYCLE()
|
|
|
|
{
|
|
|
|
solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
|
|
|
|
}
|
2012-10-09 11:25:59 +02:00
|
|
|
|
|
|
|
SANITY_CHECK(rvec, 1e-6);
|
|
|
|
SANITY_CHECK(tvec, 1e-6);
|
2012-08-10 14:39:36 +02:00
|
|
|
}
|