From 9d90b0549c15adb76ab97d18375b9e14f1b23f36 Mon Sep 17 00:00:00 2001
From: Vadim Pisarevsky <vadim.pisarevsky@gmail.com>
Date: Mon, 25 May 2015 22:03:25 +0300
Subject: [PATCH] fixed bug http://code.opencv.org/issues/3882

---
 modules/calib3d/src/solvepnp.cpp              | 11 ++++++++++-
 modules/calib3d/test/test_solvepnp_ransac.cpp | 25 +++++++++++++++++++++++++
 2 files changed, 35 insertions(+), 1 deletion(-)

diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index 233e145..e11c458 100644
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@ -192,7 +192,16 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
                         OutputArray _inliers, int flags)
 {
 
-    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
+    Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
+    Mat opoints, ipoints;
+    if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
+        opoints0.convertTo(opoints, CV_32F);
+    else
+        opoints = opoints0;
+    if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
+        ipoints0.convertTo(ipoints, CV_32F);
+    else
+        ipoints = ipoints0;
 
     int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
     CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp
index 76a3966..cb06112 100644
--- a/modules/calib3d/test/test_solvepnp_ransac.cpp
+++ b/modules/calib3d/test/test_solvepnp_ransac.cpp
@@ -319,3 +319,28 @@ TEST(DISABLED_Calib3d_SolvePnPRansac, concurrency)
 
 }
 #endif
+
+TEST(Calib3d_SolvePnP, double_support)
+{
+    Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0.,
+                       5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.);
+    std::vector<cv::Point3d> points3d;
+    std::vector<cv::Point2d> points2d;
+    std::vector<cv::Point3f> points3dF;
+    std::vector<cv::Point2f> points2dF;
+    for (int i = 0; i < 10 ; i++)
+    {
+        points3d.push_back(cv::Point3d(i,0,0));
+        points3dF.push_back(cv::Point3d(i,0,0));
+        points2d.push_back(cv::Point2f(i,0));
+        points2dF.push_back(cv::Point2d(i,0));
+    }
+    Mat R,t, RF, tF;
+    vector<int> inliers;
+
+    solvePnPRansac(points3dF, points2dF, intrinsics, cv::Mat(), RF, tF, true, 100, 8, 0.999, inliers, cv::SOLVEPNP_P3P);
+    solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R, t, true, 100, 8, 0.999, inliers, cv::SOLVEPNP_P3P);
+
+    ASSERT_LE(norm(R, Mat_<double>(RF), NORM_INF), 1e-3);
+    ASSERT_LE(norm(t, Mat_<double>(tF), NORM_INF), 1e-3);
+}
-- 
2.7.4