From d1ff87d94d9307b63276233fea2dcaf21d8e5c10 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tomasz=20O=C5=BCa=C5=84ski?= Date: Tue, 20 Jul 2021 13:49:40 +0200 Subject: [PATCH] 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. --- modules/calib3d/src/solvepnp.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) 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; }