From: Tomasz Ożański <tozanski@users.noreply.github.com>
Date: Tue, 20 Jul 2021 11:49:40 +0000 (+0200)
Subject: Bugfix for solvePnPRansac with SOLVEPNP_ITERATIVE
X-Git-Tag: accepted/tizen/unified/20230127.161057~1^2~6^2~4^2
X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=d1ff87d94d9307b63276233fea2dcaf21d8e5c10;p=platform%2Fupstream%2Fopencv.git

Bugfix for solvePnPRansac with SOLVEPNP_ITERATIVE

The current implementation overwrites the result rotation and translation in every iteration.
If SOLVEPNP_ITERATIVE was run as a refinement it will start from the incorrect initial
transformation thus  degrading the final outcome.
---

diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index ad5c85b222..21a260f720 100644
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@ -149,12 +149,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;
@@ -313,7 +314,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;
     }