From a1c5740044f575b4f35a4a4d549df429a1433553 Mon Sep 17 00:00:00 2001 From: Vijay Pradeep Date: Thu, 8 Jan 2015 18:24:55 -0800 Subject: [PATCH 1/5] solvePnPRansac now accepts a seed and is deterministic --- .../include/opencv2/calib3d/calib3d.hpp | 3 +- modules/calib3d/src/solvepnp.cpp | 56 ++++++++++--------- 2 files changed, 31 insertions(+), 28 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 5e9cde8ec0..42e1c6b67e 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -488,7 +488,8 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), - int flags = ITERATIVE); + int flags = ITERATIVE, + uint64 rng_seed = 0); //! initializes camera matrix from a few 3D points and the corresponding projections. CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 1aeb82f64b..a5c67ba6c6 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)); @@ -196,22 +196,21 @@ namespace cv } } - if (localInliers.size() > inliers.size()) + resultsMutex.lock(); + if ( (localInliers.size() > inliers.size()) || (localInliers.size() == inliers.size() && curIndex > bestIndex)) { - resultsMutex.lock(); - inliers.clear(); inliers.resize(localInliers.size()); memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size()); localRvec.copyTo(rvec); localTvec.copyTo(tvec); - - resultsMutex.unlock(); + 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); @@ -221,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); } } @@ -240,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 @@ -257,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&); @@ -274,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++) { @@ -294,7 +295,6 @@ namespace cv }; Mutex PnPSolver::syncMutex; - RNG PnPSolver::generator; } } @@ -303,7 +303,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, int minInliersCount, - OutputArray _inliers, int flags) + OutputArray _inliers, int flags, uint64 _rng_seed) { Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); @@ -336,11 +336,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) @@ -357,7 +359,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); From 177478a8ad81855e2abf187bb7451b98f6c82aca Mon Sep 17 00:00:00 2001 From: Vijay Pradeep Date: Fri, 9 Jan 2015 10:17:18 -0800 Subject: [PATCH 2/5] overload instead of default arg for ABI backwards compatibility. rng_seed is now 'int' to support python port --- .../include/opencv2/calib3d/calib3d.hpp | 18 ++++++++++++++++-- modules/calib3d/src/solvepnp.cpp | 13 ++++++++++++- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 42e1c6b67e..ce265617e0 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -488,8 +488,22 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), - int flags = ITERATIVE, - uint64 rng_seed = 0); + int flags = ITERATIVE); + +//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible. +CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, + InputArray imagePoints, + InputArray cameraMatrix, + InputArray distCoeffs, + OutputArray rvec, + OutputArray tvec, + bool useExtrinsicGuess, + int iterationsCount, + float reprojectionError, + int minInliersCount, + OutputArray inliers, + int flags, + int rng_seed); //! initializes camera matrix from a few 3D points and the corresponding projections. CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index a5c67ba6c6..2122eb7cd9 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -303,7 +303,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, int minInliersCount, - OutputArray _inliers, int flags, uint64 _rng_seed) + OutputArray _inliers, int flags, int _rng_seed) { Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); @@ -376,3 +376,14 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, } return; } + +void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, + InputArray _cameraMatrix, InputArray _distCoeffs, + OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, + int iterationsCount, float reprojectionError, int minInliersCount, + OutputArray _inliers, int flags) +{ + solvePnPRansac(_opoints, _ipoints, _cameraMatrix, _distCoeffs, + _rvec, _tvec, useExtrinsicGuess, iterationsCount, + reprojectionError, minInliersCount, _inliers, flags, 0); +} From 964d5a20e93c6b24050822d6f20482d5863c098c Mon Sep 17 00:00:00 2001 From: Vijay Pradeep Date: Fri, 9 Jan 2015 10:44:19 -0800 Subject: [PATCH 3/5] Whitespace fix --- modules/calib3d/src/solvepnp.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 2122eb7cd9..bae05d9bc2 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -383,7 +383,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, int iterationsCount, float reprojectionError, int minInliersCount, OutputArray _inliers, int flags) { - solvePnPRansac(_opoints, _ipoints, _cameraMatrix, _distCoeffs, - _rvec, _tvec, useExtrinsicGuess, iterationsCount, - reprojectionError, minInliersCount, _inliers, flags, 0); + solvePnPRansac(_opoints, _ipoints, _cameraMatrix, _distCoeffs, + _rvec, _tvec, useExtrinsicGuess, iterationsCount, + reprojectionError, minInliersCount, _inliers, flags, 0); } From 1292fdf71a9220a7d2182d21e413c0910c203c6a Mon Sep 17 00:00:00 2001 From: Vijay Pradeep Date: Tue, 13 Jan 2015 11:25:01 -0800 Subject: [PATCH 4/5] Now using default arg instead of overloads --- .../include/opencv2/calib3d/calib3d.hpp | 18 ++---------------- modules/calib3d/src/solvepnp.cpp | 11 ----------- 2 files changed, 2 insertions(+), 27 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index ce265617e0..bd579e8d28 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -488,22 +488,8 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), - int flags = ITERATIVE); - -//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible. -CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, - InputArray imagePoints, - InputArray cameraMatrix, - InputArray distCoeffs, - OutputArray rvec, - OutputArray tvec, - bool useExtrinsicGuess, - int iterationsCount, - float reprojectionError, - int minInliersCount, - OutputArray inliers, - int flags, - int rng_seed); + int flags = ITERATIVE, + int rng_seed = 0); //! initializes camera matrix from a few 3D points and the corresponding projections. CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index bae05d9bc2..44e6aaac03 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -376,14 +376,3 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, } return; } - -void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, - InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, - int iterationsCount, float reprojectionError, int minInliersCount, - OutputArray _inliers, int flags) -{ - solvePnPRansac(_opoints, _ipoints, _cameraMatrix, _distCoeffs, - _rvec, _tvec, useExtrinsicGuess, iterationsCount, - reprojectionError, minInliersCount, _inliers, flags, 0); -} From 49bc100e51dcc02a65492b14a924ea31281c4626 Mon Sep 17 00:00:00 2001 From: Vijay Pradeep Date: Tue, 17 Feb 2015 09:19:22 -0800 Subject: [PATCH 5/5] Removing rng_seed arg in order to maintain ABI compatibility. Hardcoding seed --- modules/calib3d/include/opencv2/calib3d/calib3d.hpp | 3 +-- modules/calib3d/src/solvepnp.cpp | 3 ++- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index bd579e8d28..5e9cde8ec0 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -488,8 +488,7 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), - int flags = ITERATIVE, - int rng_seed = 0); + int flags = ITERATIVE); //! initializes camera matrix from a few 3D points and the corresponding projections. CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 44e6aaac03..be587031ec 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -303,8 +303,9 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, int minInliersCount, - OutputArray _inliers, int flags, int _rng_seed) + OutputArray _inliers, int flags) { + const int _rng_seed = 0; Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();