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; }