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

Merge pull request #2948 from PhilLab:patch-1

Browse Source
This commit is contained in:
Vadim Pisarevsky 2014-07-18 10:10:44 +00:00
commit d7bbafd683
1 changed files with 36 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
modules/calib3d/src/solvepnp.cpp
View File

@ -137,11 +137,13 @@ namespace cv
CameraParameters camera; CameraParameters camera;
}; };
template <typename OpointType, typename IpointType>
static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints, static void pnpTask(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, 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_32FC3), modelImagePoints(1, MIN_POINTS_COUNT, CV_32FC2); Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth<OpointType>::value, 3));
Mat modelImagePoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth<IpointType>::value, 2));
for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++) for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
{ {
if (pointsMask[i]) if (pointsMask[i])
@ -160,7 +162,7 @@ namespace cv
for (int i = 0; i < MIN_POINTS_COUNT; i++) for (int i = 0; i < MIN_POINTS_COUNT; i++)
for (int j = i + 1; j < MIN_POINTS_COUNT; j++) for (int j = i + 1; j < MIN_POINTS_COUNT; j++)
{ {
if (norm(modelObjectPoints.at<Vec3f>(0, i) - modelObjectPoints.at<Vec3f>(0, j)) < eps) if (norm(modelObjectPoints.at<Vec<OpointType,3> >(0, i) - modelObjectPoints.at<Vec<OpointType,3> >(0, j)) < eps)
num_same_points++; num_same_points++;
} }
if (num_same_points > 0) if (num_same_points > 0)
@ -174,7 +176,7 @@ namespace cv
params.useExtrinsicGuess, params.flags); params.useExtrinsicGuess, params.flags);
vector<Point2f> projected_points; vector<Point_<OpointType> > projected_points;
projected_points.resize(objectPoints.cols); projected_points.resize(objectPoints.cols);
projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points); projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points);
@ -184,9 +186,11 @@ namespace cv
vector<int> localInliers; vector<int> localInliers;
for (int i = 0; i < objectPoints.cols; i++) for (int i = 0; i < objectPoints.cols; i++)
{ {
Point2f p(imagePoints.at<Vec2f>(0, i)[0], imagePoints.at<Vec2f>(0, i)[1]); //Although p is a 2D point it needs the same type as the object points to enable the norm calculation
Point_<OpointType> p((OpointType)imagePoints.at<Vec<IpointType,2> >(0, i)[0],
(OpointType)imagePoints.at<Vec<IpointType,2> >(0, i)[1]);
if ((norm(p - projected_points[i]) < params.reprojectionError) if ((norm(p - projected_points[i]) < params.reprojectionError)
&& (rotatedPoints.at<Vec3f>(0, i)[2] > 0)) //hack && (rotatedPoints.at<Vec<OpointType,3> >(0, i)[2] > 0)) //hack
{ {
localInliers.push_back(i); localInliers.push_back(i);
} }
@ -206,6 +210,30 @@ namespace cv
} }
} }
static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
const Parameters& params, vector<int>& inliers, Mat& rvec, Mat& tvec,
const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex)
{
CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F);
CV_Assert(imagePoints.depth() == CV_64F || imagePoints.depth() == CV_32F);
const bool objectDoublePrecision = objectPoints.depth() == CV_64F;
const bool imageDoublePrecision = imagePoints.depth() == CV_64F;
if(objectDoublePrecision)
{
if(imageDoublePrecision)
pnpTask<double, double>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex);
else
pnpTask<double, float>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex);
}
else
{
if(imageDoublePrecision)
pnpTask<float, double>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex);
else
pnpTask<float, float>(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex);
}
}
class PnPSolver class PnPSolver
{ {
public: public:
@ -281,10 +309,10 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
CV_Assert(opoints.isContinuous()); CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F); CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3); CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
CV_Assert(ipoints.isContinuous()); CV_Assert(ipoints.isContinuous());
CV_Assert(ipoints.depth() == CV_32F); CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2); CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1); _rvec.create(3, 1, CV_64FC1);
@ -320,7 +348,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
if (flags != CV_P3P) if (flags != CV_P3P)
{ {
int i, pointsCount = (int)localInliers.size(); int i, pointsCount = (int)localInliers.size();
Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2); Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
for (i = 0; i < pointsCount; i++) for (i = 0; i < pointsCount; i++)
{ {
int index = localInliers[i]; int index = localInliers[i];