From 60e33921e3c541898b04fbc3f309f45c63e2708e Mon Sep 17 00:00:00 2001
From: Alexey Spizhevoy <no@email>
Date: Tue, 1 Mar 2011 09:17:22 +0000
Subject: [PATCH] added performance sample for solvePnpRansac + refactoring

---
 modules/gpu/src/calib3d.cpp           |  1 +
 modules/gpu/test/test_calib3d.cpp     |  2 +-
 samples/gpu/performance/performance.h |  2 +-
 samples/gpu/performance/tests.cpp     | 51 +++++++++++++++++++++++++--
 4 files changed, 51 insertions(+), 5 deletions(-)

diff --git a/modules/gpu/src/calib3d.cpp b/modules/gpu/src/calib3d.cpp
index 67f9b68026..68987131cf 100644
--- a/modules/gpu/src/calib3d.cpp
+++ b/modules/gpu/src/calib3d.cpp
@@ -243,6 +243,7 @@ void cv::gpu::solvePnpRansac(const Mat& object, const Mat& image, const Mat& cam
     CV_Assert(!params.use_extrinsic_guess); // We don't support initial guess for now
 
     const int num_points = object.cols;
+    CV_Assert(num_points >= params.subset_size);
 
     // Unapply distortion and intrinsic camera transformations
     Mat eye_camera_mat = Mat::eye(3, 3, CV_32F);
diff --git a/modules/gpu/test/test_calib3d.cpp b/modules/gpu/test/test_calib3d.cpp
index 19edcddc58..d03b3d3be1 100644
--- a/modules/gpu/test/test_calib3d.cpp
+++ b/modules/gpu/test/test_calib3d.cpp
@@ -113,7 +113,7 @@ TEST(solvePnpRansac, accuracy)
 
     const int num_points = 5000;
     Mat object = randomMat(rng, Size(num_points, 1), CV_32FC3, 0, 100, false);
-    Mat camera_mat = randomMat(rng, Size(3, 3), CV_32F, 1, 1, false);
+    Mat camera_mat = randomMat(rng, Size(3, 3), CV_32F, 0.5, 1, false);
     camera_mat.at<float>(0, 1) = 0.f;
     camera_mat.at<float>(1, 0) = 0.f;
     camera_mat.at<float>(2, 0) = 0.f;
diff --git a/samples/gpu/performance/performance.h b/samples/gpu/performance/performance.h
index c698b92c52..31931e7776 100644
--- a/samples/gpu/performance/performance.h
+++ b/samples/gpu/performance/performance.h
@@ -100,7 +100,7 @@ private:
 };
 
 
-#define INIT(name) \
+#define GLOBAL_INIT(name) \
     struct name##_init: Runnable { \
         name##_init(): Runnable(#name) { \
             TestSystem::instance().addInit(this); \
diff --git a/samples/gpu/performance/tests.cpp b/samples/gpu/performance/tests.cpp
index a4db160869..16e3dc3067 100644
--- a/samples/gpu/performance/tests.cpp
+++ b/samples/gpu/performance/tests.cpp
@@ -8,19 +8,19 @@
 using namespace std;
 using namespace cv;
 
-INIT(matchTemplate)
+void InitMatchTemplate()
 {
     Mat src; gen(src, 500, 500, CV_32F, 0, 1);
     Mat templ; gen(templ, 500, 500, CV_32F, 0, 1);
-
     gpu::GpuMat d_src(src), d_templ(templ), d_dst;
-
     gpu::matchTemplate(d_src, d_templ, d_dst, CV_TM_CCORR);
 }
 
 
 TEST(matchTemplate)
 {
+    InitMatchTemplate();
+
     Mat src, templ, dst;
     gen(src, 3000, 3000, CV_32F, 0, 1);
 
@@ -780,3 +780,48 @@ TEST(projectPoints)
         GPU_OFF;
     }
 }
+
+
+void InitSolvePnpRansac()
+{
+    Mat object; gen(object, 1, 4, CV_32FC3, Scalar::all(0), Scalar::all(100));
+    Mat image; gen(image, 1, 4, CV_32FC2, Scalar::all(0), Scalar::all(100));
+    Mat rvec, tvec;
+    gpu::solvePnpRansac(object, image, Mat::eye(3, 3, CV_32F), Mat(), rvec, tvec, 
+                        gpu::SolvePnpRansacParams());
+}
+
+
+// It's not very correct test as solvePnP and solvePnpRansac use different algorithms internally
+// TODO add proper test after CPU solvePnpRansac being added
+TEST(solvePnpRansac)
+{
+    InitSolvePnpRansac();
+
+    int num_points = 1000000;
+
+    Mat object; gen(object, 1, num_points, CV_32FC3, Scalar::all(0), Scalar::all(100));
+    Mat camera_mat; gen(camera_mat, 3, 3, CV_32F, 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 rvec_gold; gen(rvec_gold, 1, 3, CV_32F, 0, 1);
+    Mat tvec_gold; gen(tvec_gold, 1, 3, CV_32F, 0, 1);
+
+    vector<Point2f> image_vec;
+    projectPoints(object, rvec_gold, tvec_gold, camera_mat, Mat(), image_vec);
+    Mat image(1, image_vec.size(), CV_32FC2, &image_vec[0]);
+
+    Mat rvec, tvec;
+
+    CPU_ON;
+    solvePnP(object, image, camera_mat, Mat(), rvec, tvec);
+    CPU_OFF;
+
+    GPU_ON;
+    gpu::SolvePnpRansacParams params;
+    gpu::solvePnpRansac(object, image, camera_mat, Mat(), rvec, tvec, params);
+    GPU_OFF;
+}
\ No newline at end of file