generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #3588 from vpradeep07:feature/deterministic_pnpransac

Browse Source
This commit is contained in:
Vadim Pisarevsky 2015-02-20 12:32:35 +00:00
commit 85fd599939
1 changed files with 27 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
modules/calib3d/src/solvepnp.cpp
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));
@ -197,19 +197,20 @@ namespace cv
} }
resultsMutex.lock(); resultsMutex.lock();
if (localInliers.size() > inliers.size()) if ( (localInliers.size() > inliers.size()) || (localInliers.size() == inliers.size() && curIndex > bestIndex))
{ {
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);
@ -219,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);
} }
} }
@ -238,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
@ -255,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&);
@ -272,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++)
{ {
@ -292,7 +295,6 @@ namespace cv
}; };
Mutex PnPSolver::syncMutex; Mutex PnPSolver::syncMutex;
RNG PnPSolver::generator;
} }
} }
@ -303,6 +305,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
int iterationsCount, float reprojectionError, int minInliersCount, int iterationsCount, float reprojectionError, int minInliersCount,
OutputArray _inliers, int flags) OutputArray _inliers, int flags)
{ {
const int _rng_seed = 0;
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();
@ -334,11 +337,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)
@ -355,7 +360,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);