From 1d62fddd31149d4558d71674f021969944e60184 Mon Sep 17 00:00:00 2001
From: Alexey Spizhevoy <no@email>
Date: Mon, 7 Mar 2011 13:36:47 +0000
Subject: [PATCH] updated solvePnpRansac performance test

---
 modules/gpu/src/calib3d.cpp       |  3 +++
 modules/gpu/src/cuda/calib3d.cu   | 11 ++++++---
 samples/gpu/performance/tests.cpp | 50 +++++++++++++++++++++------------------
 3 files changed, 38 insertions(+), 26 deletions(-)

diff --git a/modules/gpu/src/calib3d.cpp b/modules/gpu/src/calib3d.cpp
index 7530f37..c0374f2 100644
--- a/modules/gpu/src/calib3d.cpp
+++ b/modules/gpu/src/calib3d.cpp
@@ -145,6 +145,8 @@ void cv::gpu::projectPoints(const GpuMat& src, const Mat& rvec, const Mat& tvec,
 
 namespace cv { namespace gpu { namespace solve_pnp_ransac
 {
+    int maxNumIters();
+
     void computeHypothesisScores(
             const int num_hypotheses, const int num_points, const float* rot_matrices,
             const float3* transl_vectors, const float3* object, const float2* image,
@@ -241,6 +243,7 @@ void cv::gpu::solvePnpRansac(const Mat& object, const Mat& image, const Mat& cam
     CV_Assert(object.cols == image.cols);
     CV_Assert(camera_mat.size() == Size(3, 3) && camera_mat.type() == CV_32F);
     CV_Assert(!params.use_extrinsic_guess); // We don't support initial guess for now
+    CV_Assert(params.num_iters <= solve_pnp_ransac::maxNumIters());
 
     const int num_points = object.cols;
     CV_Assert(num_points >= params.subset_size);
diff --git a/modules/gpu/src/cuda/calib3d.cu b/modules/gpu/src/cuda/calib3d.cu
index 171fc8c..200b82a 100644
--- a/modules/gpu/src/cuda/calib3d.cu
+++ b/modules/gpu/src/cuda/calib3d.cu
@@ -43,7 +43,7 @@
 #include "internal_shared.hpp"
 #include "opencv2/gpu/device/transform.hpp"
 
-#define SOLVE_PNP_RANSAC_NUM_ITERS 200
+#define SOLVE_PNP_RANSAC_MAX_NUM_ITERS 200
 
 namespace cv { namespace gpu
 {
@@ -120,8 +120,13 @@ namespace cv { namespace gpu
 
     namespace solve_pnp_ransac
     {
-        __constant__ float3 crot_matrices[SOLVE_PNP_RANSAC_NUM_ITERS * 3];
-        __constant__ float3 ctransl_vectors[SOLVE_PNP_RANSAC_NUM_ITERS];
+        __constant__ float3 crot_matrices[SOLVE_PNP_RANSAC_MAX_NUM_ITERS * 3];
+        __constant__ float3 ctransl_vectors[SOLVE_PNP_RANSAC_MAX_NUM_ITERS];
+
+        int maxNumIters()
+        {
+            return SOLVE_PNP_RANSAC_MAX_NUM_ITERS;
+        }
 
         __device__ float sqr(float x)
         {
diff --git a/samples/gpu/performance/tests.cpp b/samples/gpu/performance/tests.cpp
index 16e3dc3..39fc34f 100644
--- a/samples/gpu/performance/tests.cpp
+++ b/samples/gpu/performance/tests.cpp
@@ -792,36 +792,40 @@ void InitSolvePnpRansac()
 }
 
 
-// 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;
+    for (int num_points = 5000; num_points <= 300000; num_points = int(num_points * 3.76))
+    {
+        SUBTEST << "num_points " << num_points;
 
-    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 object; gen(object, 1, num_points, CV_32FC3, Scalar::all(10), Scalar::all(100));
+        Mat image; gen(image, 1, num_points, CV_32FC2, Scalar::all(10), 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);
+        Mat rvec, tvec;
+        const int num_iters = 200;
+        const float max_dist = 2.0f;
+        vector<int> inliers_cpu;
 
-    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]);
+        CPU_ON;
+        solvePnPRansac(object, image, camera_mat, Mat(), rvec, tvec, false, num_iters, 
+                       max_dist, int(num_points * 0.05), &inliers_cpu);
+        CPU_OFF;
 
-    Mat rvec, tvec;
-
-    CPU_ON;
-    solvePnP(object, image, camera_mat, Mat(), rvec, tvec);
-    CPU_OFF;
+        gpu::SolvePnpRansacParams params;
+        params.num_iters = num_iters;
+        params.max_dist = max_dist;
+        vector<int> inliers_gpu;
+        params.inliers = &inliers_gpu;
 
-    GPU_ON;
-    gpu::SolvePnpRansacParams params;
-    gpu::solvePnpRansac(object, image, camera_mat, Mat(), rvec, tvec, params);
-    GPU_OFF;
+        GPU_ON;
+        gpu::solvePnpRansac(object, image, camera_mat, Mat(), rvec, tvec, params);
+        GPU_OFF;
+    }
 }
\ No newline at end of file
-- 
2.7.4