Merge pull request opencv#22914 from tozanski:tomoz/ransac-bugfix

Bugfix for solvePnPRansac with SOLVEPNP_ITERATIVE
advait-0 · Dec 22, 2022 · 44dfe62 · 44dfe62
2 parents d44c58a + d1ff87d
commit 44dfe62
Showing 1 changed file with 11 additions and 4 deletions.
diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
@@ -149,12 +149,13 @@ class PnPRansacCallback CV_FINAL : public PointSetRegistrator::Callback
     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;
     }