diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index f00958d055..be587031ec 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -138,8 +138,8 @@ namespace cv }; template - static void pnpTask(const vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, - const Parameters& params, vector& inliers, Mat& rvec, Mat& tvec, + static void pnpTask(const int curIndex, const vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, + const Parameters& params, vector& inliers, int& bestIndex, Mat& rvec, Mat& tvec, const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) { Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth::value, 3)); @@ -197,19 +197,20 @@ namespace cv } resultsMutex.lock(); - if (localInliers.size() > inliers.size()) + if ( (localInliers.size() > inliers.size()) || (localInliers.size() == inliers.size() && curIndex > bestIndex)) { inliers.clear(); inliers.resize(localInliers.size()); memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size()); localRvec.copyTo(rvec); localTvec.copyTo(tvec); + bestIndex = curIndex; } resultsMutex.unlock(); } - static void pnpTask(const vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, - const Parameters& params, vector& inliers, Mat& rvec, Mat& tvec, + static void pnpTask(const int curIndex, const vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, + const Parameters& params, vector& inliers, int& bestIndex, Mat& rvec, Mat& tvec, const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) { CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F); @@ -219,16 +220,16 @@ namespace cv if(objectDoublePrecision) { if(imageDoublePrecision) - pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + pnpTask(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex); else - pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + pnpTask(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex); } else { if(imageDoublePrecision) - pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + pnpTask(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex); else - pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + pnpTask(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex); } } @@ -238,12 +239,13 @@ namespace cv void operator()( const BlockedRange& r ) const { vector pointsMask(objectPoints.cols, 0); - memset(&pointsMask[0], 1, MIN_POINTS_COUNT ); for( int i=r.begin(); i!=r.end(); ++i ) { - generateVar(pointsMask); - pnpTask(pointsMask, objectPoints, imagePoints, parameters, - inliers, rvec, tvec, initRvec, initTvec, syncMutex); + memset(&pointsMask[0], 0, objectPoints.cols ); + memset(&pointsMask[0], 1, MIN_POINTS_COUNT ); + generateVar(pointsMask, rng_base_seed + i); + pnpTask(i, pointsMask, objectPoints, imagePoints, parameters, + inliers, bestIndex, rvec, tvec, initRvec, initTvec, syncMutex); if ((int)inliers.size() >= parameters.minInliersCount) { #ifdef HAVE_TBB @@ -255,14 +257,13 @@ namespace cv } } PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters, - Mat& _rvec, Mat& _tvec, vector& _inliers): + Mat& _rvec, Mat& _tvec, vector& _inliers, int& _bestIndex, uint64 _rng_base_seed): objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters), - rvec(_rvec), tvec(_tvec), inliers(_inliers) + rvec(_rvec), tvec(_tvec), inliers(_inliers), bestIndex(_bestIndex), rng_base_seed(_rng_base_seed) { + bestIndex = -1; rvec.copyTo(initRvec); tvec.copyTo(initTvec); - - generator.state = theRNG().state; //to control it somehow... } private: PnPSolver& operator=(const PnPSolver&); @@ -272,13 +273,15 @@ namespace cv const Parameters& parameters; Mat &rvec, &tvec; vector& inliers; + int& bestIndex; + const uint64 rng_base_seed; Mat initRvec, initTvec; - static RNG generator; static Mutex syncMutex; - void generateVar(vector& mask) const + void generateVar(vector& mask, uint64 rng_seed) const { + RNG generator(rng_seed); int size = (int)mask.size(); for (int i = 0; i < size; i++) { @@ -292,7 +295,6 @@ namespace cv }; Mutex PnPSolver::syncMutex; - RNG PnPSolver::generator; } } @@ -303,6 +305,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, int iterationsCount, float reprojectionError, int minInliersCount, OutputArray _inliers, int flags) { + const int _rng_seed = 0; Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); @@ -334,11 +337,13 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, Mat localRvec, localTvec; rvec.copyTo(localRvec); tvec.copyTo(localTvec); + int bestIndex; if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT) { parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params, - localRvec, localTvec, localInliers)); + localRvec, localTvec, localInliers, bestIndex, + _rng_seed)); } if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT) @@ -355,7 +360,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1)); objectPoints.col(index).copyTo(colInlierObjectPoints); } - solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags); + solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags); } localRvec.copyTo(rvec); localTvec.copyTo(tvec);