Merge remote-tracking branch 'upstream/3.4' into merge-3.4

[platform/upstream/opencv.git] / modules / calib3d / src / solvepnp.cpp

diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index 07780c4..d9e2664 100644 (file)
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@ -152,12 +152,13 @@ public:
     int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
     {
         Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
-
+        Mat iter_rvec = rvec.clone();
+        Mat iter_tvec = tvec.clone();
         bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
-                                            rvec, tvec, useExtrinsicGuess, flags );
+                                            iter_rvec, iter_tvec, useExtrinsicGuess, flags );
 
         Mat _local_model;
-        hconcat(rvec, tvec, _local_model);
+        hconcat(iter_rvec, iter_tvec, _local_model);
         _local_model.copyTo(_model);
 
         return correspondence;
@@ -336,7 +337,13 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
     ipoints_inliers.resize(npoints1);
     try
     {
-        result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
+       if (flags == SOLVEPNP_ITERATIVE && !useExtrinsicGuess)
+       {
+          rvec = _local_model.col(0).clone();
+          tvec = _local_model.col(1).clone();
+          useExtrinsicGuess = true;
+       }
+       result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
                           distCoeffs, rvec, tvec, useExtrinsicGuess,
                           (flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
     }