Chiebot-Mirror
/
opencv
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

solvePnPRansac now accepts a seed and is deterministic

Browse Source
pull/3588/head
Vijay Pradeep 10 years ago
parent 4e9cb475da
commit a1c5740044
2 changed files with 31 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      modules/calib3d/include/opencv2/calib3d/calib3d.hpp
  2. 56
      modules/calib3d/src/solvepnp.cpp

3
modules/calib3d/include/opencv2/calib3d/calib3d.hpp
Unescape View File

@ -488,7 +488,8 @@ CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints,
float reprojectionError = 8.0, float reprojectionError = 8.0,
int minInliersCount = 100, int minInliersCount = 100,
OutputArray inliers = noArray(), 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. //! initializes camera matrix from a few 3D points and the corresponding projections.
CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints,

56
modules/calib3d/src/solvepnp.cpp
Unescape View File

@ -138,8 +138,8 @@ namespace cv
}; };
template <typename OpointType, typename IpointType> template <typename OpointType, typename IpointType>
static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints, static void pnpTask(const int curIndex, const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, vector<int>& inliers, Mat& rvec, Mat& tvec, const Parameters& params, vector<int>& inliers, int& bestIndex, Mat& rvec, Mat& tvec,
const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex)
{ {
Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth<OpointType>::value, 3)); Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth<OpointType>::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.clear();
inliers.resize(localInliers.size()); inliers.resize(localInliers.size());
memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size()); memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
localRvec.copyTo(rvec); localRvec.copyTo(rvec);
localTvec.copyTo(tvec); localTvec.copyTo(tvec);
bestIndex = curIndex;
resultsMutex.unlock();
} }
resultsMutex.unlock();
} }
static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints, static void pnpTask(const int curIndex, const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, vector<int>& inliers, Mat& rvec, Mat& tvec, const Parameters& params, vector<int>& inliers, int& bestIndex, Mat& rvec, Mat& tvec,
const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex)
{ {
CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F); CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F);
@ -221,16 +220,16 @@ namespace cv
if(objectDoublePrecision) if(objectDoublePrecision)
{ {
if(imageDoublePrecision) if(imageDoublePrecision)
pnpTask<double, double>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); pnpTask<double, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
else else
pnpTask<double, float>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); pnpTask<double, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
} }
else else
{ {
if(imageDoublePrecision) if(imageDoublePrecision)
pnpTask<float, double>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); pnpTask<float, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
else else
pnpTask<float, float>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); pnpTask<float, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
} }
} }
@ -240,12 +239,13 @@ namespace cv
void operator()( const BlockedRange& r ) const void operator()( const BlockedRange& r ) const
{ {
vector<char> pointsMask(objectPoints.cols, 0); vector<char> pointsMask(objectPoints.cols, 0);
memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
for( int i=r.begin(); i!=r.end(); ++i ) for( int i=r.begin(); i!=r.end(); ++i )
{ {
generateVar(pointsMask); memset(&pointsMask[0], 0, objectPoints.cols );
pnpTask(pointsMask, objectPoints, imagePoints, parameters, memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
inliers, rvec, tvec, initRvec, initTvec, syncMutex); 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) if ((int)inliers.size() >= parameters.minInliersCount)
{ {
#ifdef HAVE_TBB #ifdef HAVE_TBB
@ -257,14 +257,13 @@ namespace cv
} }
} }
PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters, PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters,
Mat& _rvec, Mat& _tvec, vector<int>& _inliers): Mat& _rvec, Mat& _tvec, vector<int>& _inliers, int& _bestIndex, uint64 _rng_base_seed):
objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters), 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); rvec.copyTo(initRvec);
tvec.copyTo(initTvec); tvec.copyTo(initTvec);
generator.state = theRNG().state; //to control it somehow...
} }
private: private:
PnPSolver& operator=(const PnPSolver&); PnPSolver& operator=(const PnPSolver&);
@ -274,13 +273,15 @@ namespace cv
const Parameters& parameters; const Parameters& parameters;
Mat &rvec, &tvec; Mat &rvec, &tvec;
vector<int>& inliers; vector<int>& inliers;
int& bestIndex;
const uint64 rng_base_seed;
Mat initRvec, initTvec; Mat initRvec, initTvec;
static RNG generator;
static Mutex syncMutex; static Mutex syncMutex;
void generateVar(vector<char>& mask) const void generateVar(vector<char>& mask, uint64 rng_seed) const
{ {
RNG generator(rng_seed);
int size = (int)mask.size(); int size = (int)mask.size();
for (int i = 0; i < size; i++) for (int i = 0; i < size; i++)
{ {
@ -294,7 +295,6 @@ namespace cv
}; };
Mutex PnPSolver::syncMutex; Mutex PnPSolver::syncMutex;
RNG PnPSolver::generator;
} }
} }
@ -303,7 +303,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, int minInliersCount, 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 opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
@ -336,11 +336,13 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
Mat localRvec, localTvec; Mat localRvec, localTvec;
rvec.copyTo(localRvec); rvec.copyTo(localRvec);
tvec.copyTo(localTvec); tvec.copyTo(localTvec);
int bestIndex;
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT) if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
{ {
parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params, 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) 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)); Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints); 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); localRvec.copyTo(rvec);
localTvec.copyTo(tvec); localTvec.copyTo(tvec);

Write Preview
Loading…
Cancel
Save