Commit aafda43d authored by PhilLab's avatar PhilLab

Double precision for solvePnPRansac()

solvePnPRansac() and pnpTask() now accept object or image points with double precision.
parent 39127d94
No related merge requests found
...@@ -137,11 +137,13 @@ namespace cv ...@@ -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 ...@@ -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 ...@@ -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,10 @@ namespace cv ...@@ -184,9 +186,10 @@ 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(imagePoints.at<Vec<IpointType,2>>(0, i)[0], 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 +209,30 @@ namespace cv ...@@ -206,6 +209,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 +308,10 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, ...@@ -281,10 +308,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 +347,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, ...@@ -320,7 +347,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];
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment