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

added P3P method

Browse Source 
added test for solvePnP
changed test for solvePnPRansac
fixed bug with mutex solvePnPRansac
This commit is contained in:
Alexander Shishkov
2011-12-26 12:59:07 +00:00
parent 6d8f4c6b82
commit c11551a510
8 changed files with 941 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

232
modules/calib3d/test/test_solvepnp_ransac.cpp
View File

@@ -48,100 +48,194 @@ using namespace std;
class CV_solvePnPRansac_Test : public cvtest::BaseTest
{
public:
CV_solvePnPRansac_Test() {}
CV_solvePnPRansac_Test()
{
eps[CV_ITERATIVE] = 1.0e-2;
eps[CV_EPNP] = 1.0e-2;
eps[CV_P3P] = 1.0e-2;
totalTestsCount = 10;
}
~CV_solvePnPRansac_Test() {}
protected:
void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1,
-1, 5), Point3f pmax = Point3f(1, 1, 10))
-1, 5), Point3f pmax = Point3f(1, 1, 10))
{
const Point3f delta = pmax - pmin;
for (size_t i = 0; i < points.size(); i++)
const Point3f delta = pmax - pmin;
for (size_t i = 0; i < points.size(); i++)
{
Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX,
float(rand()) / RAND_MAX);
p.x *= delta.x;
p.y *= delta.y;
p.z *= delta.z;
p = p + pmin;
points[i] = p;
}
}
void generateCameraMatrix(Mat& cameraMatrix, RNG& rng)
{
const double fcMinVal = 1e-3;
const double fcMaxVal = 100;
cameraMatrix.create(3, 3, CV_64FC1);
cameraMatrix.setTo(Scalar(0));
cameraMatrix.at<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal);
cameraMatrix.at<double>(2,2) = 1;
}
void generateDistCoeffs(Mat& distCoeffs, RNG& rng)
{
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
for (int i = 0; i < 3; i++)
distCoeffs.at<double>(i,0) = rng.uniform(0.0, 1.0e-6);
}
void generatePose(Mat& rvec, Mat& tvec, RNG& rng)
{
const double minVal = 1.0e-3;
const double maxVal = 1.0;
rvec.create(3, 1, CV_64FC1);
tvec.create(3, 1, CV_64FC1);
for (int i = 0; i < 3; i++)
{
rvec.at<double>(i,0) = rng.uniform(minVal, maxVal);
tvec.at<double>(i,0) = rng.uniform(minVal, maxVal/10);
}
}
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* eps, double& maxError)
{
Mat rvec, tvec;
vector<int> inliers;
Mat trueRvec, trueTvec;
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics, rng);
if (mode == 0)
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
else
generateDistCoeffs(distCoeffs, rng);
generatePose(trueRvec, trueTvec, rng);
vector<Point2f> projectedPoints;
projectedPoints.resize(points.size());
projectPoints(Mat(points), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
for (size_t i = 0; i < projectedPoints.size(); i++)
{
if (i % 20 == 0)
{
Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX,
float(rand()) / RAND_MAX);
p.x *= delta.x;
p.y *= delta.y;
p.z *= delta.z;
p = p + pmin;
points[i] = p;
projectedPoints[i] = projectedPoints[rng.uniform(0,points.size()-1)];
}
}
solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
false, 500, 0.5, -1, inliers, method);
bool isTestSuccess = inliers.size() >= points.size()*0.95;
double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
isTestSuccess = isTestSuccess && rvecDiff < eps[method] && tvecDiff < eps[method];
double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
//cout << error << " " << inliers.size() << " " << eps[method] << endl;
if (error > maxError)
maxError = error;
return isTestSuccess;
}
void run(int)
{
cvtest::TS& ts = *this->ts;
ts.set_failed_test_info(cvtest::TS::OK);
Mat intrinsics = Mat::eye(3, 3, CV_32FC1);
intrinsics.at<float> (0, 0) = 400.0;
intrinsics.at<float> (1, 1) = 400.0;
intrinsics.at<float> (0, 2) = 640 / 2;
intrinsics.at<float> (1, 2) = 480 / 2;
Mat dist_coeffs = Mat::zeros(5, 1, CV_32FC1);
Mat rvec1 = Mat::zeros(3, 1, CV_64FC1);
Mat tvec1 = Mat::zeros(3, 1, CV_64FC1);
rvec1.at<double> (0, 0) = 1.0f;
tvec1.at<double> (0, 0) = 1.0f;
tvec1.at<double> (1, 0) = 1.0f;
vector<Point3f> points;
points.resize(500);
const int pointsCount = 500;
points.resize(pointsCount);
generate3DPointCloud(points);
vector<Point2f> points1;
points1.resize(points.size());
projectPoints(Mat(points), rvec1, tvec1, intrinsics, dist_coeffs, points1);
for (size_t i = 0; i < points1.size(); i++)
const int methodsCount = 3;
RNG rng = ts.get_rng();
for (int mode = 0; mode < 2; mode++)
{
if (i % 20 == 0)
for (int method = 0; method < methodsCount; method++)
{
points1[i] = points1[rand() % points.size()];
}
}
double eps = 1.0e-7;
int successfulTestsCount = 0;
int totalTestsCount = 10;
for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
{
try
{
Mat rvec, tvec;
vector<int> inliers;
solvePnPRansac(points, points1, intrinsics, dist_coeffs, rvec, tvec,
false, 1000, 2.0, -1, inliers);
bool isTestSuccess = inliers.size() == 475;
isTestSuccess = isTestSuccess
&& (abs(rvec.at<double> (0, 0) - 1) < eps);
isTestSuccess = isTestSuccess && (abs(rvec.at<double> (1, 0)) < eps);
isTestSuccess = isTestSuccess && (abs(rvec.at<double> (2, 0)) < eps);
isTestSuccess = isTestSuccess
&& (abs(tvec.at<double> (0, 0) - 1) < eps);
isTestSuccess = isTestSuccess
&& (abs(tvec.at<double> (1, 0) - 1) < eps);
isTestSuccess = isTestSuccess && (abs(tvec.at<double> (2, 0)) < eps);
if (isTestSuccess)
successfulTestsCount++;
}
catch(...)
{
ts.printf(cvtest::TS::LOG, "Exception in solvePnPRansac\n");
double maxError = 0;
int successfulTestsCount = 0;
for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
{
if (runTest(rng, mode, method, points, eps, maxError))
successfulTestsCount++;
}
//cout << maxError << " " << successfulTestsCount << endl;
if (successfulTestsCount < 0.7*totalTestsCount)
{
ts.printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n",
method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode);
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
}
}
}
double eps[3];
int totalTestsCount;
};
if (successfulTestsCount < 0.8*totalTestsCount)
class CV_solvePnP_Test : public CV_solvePnPRansac_Test
{
public:
CV_solvePnP_Test()
{
eps[CV_ITERATIVE] = 1.0e-6;
eps[CV_EPNP] = 1.0e-6;
eps[CV_P3P] = 1.0e-4;
totalTestsCount = 1000;
}
~CV_solvePnP_Test() {}
protected:
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* eps, double& maxError)
{
Mat rvec, tvec;
Mat trueRvec, trueTvec;
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics, rng);
if (mode == 0)
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
else
generateDistCoeffs(distCoeffs, rng);
generatePose(trueRvec, trueTvec, rng);
std::vector<Point3f> opoints;
if (method == 2)
{
ts.printf( cvtest::TS::LOG, "Invalid accuracy, failed %d tests from %d\n",
totalTestsCount - successfulTestsCount, totalTestsCount);
ts.set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
}
else
opoints = points;
vector<Point2f> projectedPoints;
projectedPoints.resize(opoints.size());
projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
false, method);
double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
bool isTestSuccess = rvecDiff < eps[method] && tvecDiff < eps[method];
double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
if (error > maxError)
maxError = error;
return isTestSuccess;
}
};
TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }