From a12ec66a049c8cc64f4f2be1983213cb0861f0e8 Mon Sep 17 00:00:00 2001
From: Vadim Pisarevsky <vadim.pisarevsky@gmail.com>
Date: Mon, 25 May 2015 22:40:10 +0300
Subject: [PATCH] fixed useExtrinsicGuess=true case with single-precision input
 (http://code.opencv.org/issues/2734)

---
 modules/calib3d/src/solvepnp.cpp              | 45 ++++++++++++++-----
 modules/calib3d/test/test_solvepnp_ransac.cpp | 40 +++++++++++++++++
 2 files changed, 74 insertions(+), 11 deletions(-)

diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index e11c4588b2..8b20cf7e46 100644
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@ -59,9 +59,33 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
     Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
     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)) );
-    _rvec.create(3, 1, CV_64F);
-    _tvec.create(3, 1, CV_64F);
-    Mat cameraMatrix = Mat_<double>(_cameraMatrix.getMat()), distCoeffs = Mat_<double>(_distCoeffs.getMat());
+
+    Mat rvec, tvec;
+    if( flags != SOLVEPNP_ITERATIVE )
+        useExtrinsicGuess = false;
+
+    if( useExtrinsicGuess )
+    {
+        int rtype = _rvec.type(), ttype = _tvec.type();
+        Size rsize = _rvec.size(), tsize = _tvec.size();
+        CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
+                   (ttype == CV_32F || ttype == CV_64F) );
+        CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
+                   (tsize == Size(1, 3) || tsize == Size(3, 1)) );
+    }
+    else
+    {
+        _rvec.create(3, 1, CV_64F);
+        _tvec.create(3, 1, CV_64F);
+    }
+    rvec = _rvec.getMat();
+    tvec = _tvec.getMat();
+
+    Mat cameraMatrix0 = _cameraMatrix.getMat();
+    Mat distCoeffs0 = _distCoeffs.getMat();
+    Mat cameraMatrix = Mat_<double>(cameraMatrix0);
+    Mat distCoeffs = Mat_<double>(distCoeffs0);
+    bool result = false;
 
     if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
     {
@@ -69,10 +93,10 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
         undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
         epnp PnP(cameraMatrix, opoints, undistortedPoints);
 
-        Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
+        Mat R;
         PnP.compute_pose(R, tvec);
         Rodrigues(R, rvec);
-        return true;
+        result = true;
     }
     else if (flags == SOLVEPNP_P3P)
     {
@@ -81,21 +105,20 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
         undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
         p3p P3Psolver(cameraMatrix);
 
-        Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
-        bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
+        Mat R;
+        result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
         if (result)
             Rodrigues(R, rvec);
-        return result;
     }
     else if (flags == SOLVEPNP_ITERATIVE)
     {
         CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
         CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
-        CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
+        CvMat c_rvec = rvec, c_tvec = tvec;
         cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
                                      c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
                                      &c_rvec, &c_tvec, useExtrinsicGuess );
-        return true;
+        result = true;
     }
     /*else if (flags == SOLVEPNP_DLS)
     {
@@ -121,7 +144,7 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
     }*/
     else
         CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
-    return false;
+    return result;
 }
 
 class PnPRansacCallback : public PointSetRegistrator::Callback
diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp
index cb0611246d..a3dbd16775 100644
--- a/modules/calib3d/test/test_solvepnp_ransac.cpp
+++ b/modules/calib3d/test/test_solvepnp_ransac.cpp
@@ -344,3 +344,43 @@ TEST(Calib3d_SolvePnP, double_support)
     ASSERT_LE(norm(R, Mat_<double>(RF), NORM_INF), 1e-3);
     ASSERT_LE(norm(t, Mat_<double>(tF), NORM_INF), 1e-3);
 }
+
+TEST(Calib3d_SolvePnP, translation)
+{
+    Mat cameraIntrinsic = Mat::eye(3,3, CV_32FC1);
+    vector<float> crvec;
+    crvec.push_back(0.f);
+    crvec.push_back(0.f);
+    crvec.push_back(0.f);
+    vector<float> ctvec;
+    ctvec.push_back(100.f);
+    ctvec.push_back(100.f);
+    ctvec.push_back(0.f);
+    vector<Point3f> p3d;
+    p3d.push_back(Point3f(0,0,0));
+    p3d.push_back(Point3f(0,0,10));
+    p3d.push_back(Point3f(0,10,10));
+    p3d.push_back(Point3f(10,10,10));
+    p3d.push_back(Point3f(2,5,5));
+
+    vector<Point2f> p2d;
+    projectPoints(p3d, crvec, ctvec, cameraIntrinsic, noArray(), p2d);
+    Mat rvec;
+    Mat tvec;
+    rvec =(Mat_<float>(3,1) << 0, 0, 0);
+    tvec = (Mat_<float>(3,1) << 100, 100, 0);
+
+    solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true);
+    ASSERT_TRUE(checkRange(rvec));
+    ASSERT_TRUE(checkRange(tvec));
+
+    rvec =(Mat_<double>(3,1) << 0, 0, 0);
+    tvec = (Mat_<double>(3,1) << 100, 100, 0);
+    solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true);
+    ASSERT_TRUE(checkRange(rvec));
+    ASSERT_TRUE(checkRange(tvec));
+
+    solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, false);
+    ASSERT_TRUE(checkRange(rvec));
+    ASSERT_TRUE(checkRange(tvec));
+}