Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #3588 from vpradeep07:feature/deterministic_pnpransac

Browse Source
pull/3737/head
Vadim Pisarevsky 10 years ago
parent f3210b58a1 4ce31c7c1a
commit 85fd599939
1 changed files with 27 additions and 22 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 49
      modules/calib3d/src/solvepnp.cpp

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

@ -138,8 +138,8 @@ namespace cv
};
template <typename OpointType, typename IpointType>
static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, vector<int>& inliers, Mat& rvec, Mat& tvec,
static void pnpTask(const int curIndex, const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, vector<int>& inliers, int& bestIndex, Mat& rvec, Mat& tvec,
const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex)
{
Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth<OpointType>::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<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, vector<int>& inliers, Mat& rvec, Mat& tvec,
static void pnpTask(const int curIndex, const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, vector<int>& 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<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
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
{
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
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
{
vector<char> 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<int>& _inliers):
Mat& _rvec, Mat& _tvec, vector<int>& _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<int>& inliers;
int& bestIndex;
const uint64 rng_base_seed;
Mat initRvec, initTvec;
static RNG generator;
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();
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);

Write Preview
Loading…
Cancel
Save