medvedych/cameracv
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity
cameracv/libs/opencv/modules/calib3d/test/test_solvepnp_ransac.cpp

2262 lines
89 KiB
C++
Raw Permalink Normal View History

initial commit
2023-05-18 21:39:43 +03:00
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "test_precomp.hpp"
namespace opencv_test { namespace {
//Statistics Helpers
struct ErrorInfo
{
ErrorInfo(double errT, double errR) : errorTrans(errT), errorRot(errR)
{
}
bool operator<(const ErrorInfo& e) const
{
return sqrt(errorTrans*errorTrans + errorRot*errorRot) <
sqrt(e.errorTrans*e.errorTrans + e.errorRot*e.errorRot);
}
double errorTrans;
double errorRot;
};
//Try to find the translation and rotation thresholds to achieve a predefined percentage of success.
//Since a success is defined by error_trans < trans_thresh && error_rot < rot_thresh
//this just gives an idea of the values to use
static void findThreshold(const std::vector<double>& v_trans, const std::vector<double>& v_rot, double percentage,
double& transThresh, double& rotThresh)
{
if (v_trans.empty() || v_rot.empty() || v_trans.size() != v_rot.size())
{
transThresh = -1;
rotThresh = -1;
return;
}
std::vector<ErrorInfo> error_info;
error_info.reserve(v_trans.size());
for (size_t i = 0; i < v_trans.size(); i++)
{
error_info.push_back(ErrorInfo(v_trans[i], v_rot[i]));
}
std::sort(error_info.begin(), error_info.end());
size_t idx = static_cast<size_t>(error_info.size() * percentage);
transThresh = error_info[idx].errorTrans;
rotThresh = error_info[idx].errorRot;
}
static double getMax(const std::vector<double>& v)
{
return *std::max_element(v.begin(), v.end());
}
static double getMean(const std::vector<double>& v)
{
if (v.empty())
{
return 0.0;
}
double sum = std::accumulate(v.begin(), v.end(), 0.0);
return sum / v.size();
}
static double getMedian(const std::vector<double>& v)
{
if (v.empty())
{
return 0.0;
}
std::vector<double> v_copy = v;
size_t size = v_copy.size();
size_t n = size / 2;
std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
double val_n = v_copy[n];
if (size % 2 == 1)
{
return val_n;
} else
{
std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
return 0.5 * (val_n + v_copy[n - 1]);
}
}
static void generatePose(const vector<Point3d>& points, Mat& rvec, Mat& tvec, RNG& rng, int nbTrials=10)
{
const double minVal = 1.0e-3;
const double maxVal = 1.0;
rvec.create(3, 1, CV_64FC1);
tvec.create(3, 1, CV_64FC1);
bool validPose = false;
for (int trial = 0; trial < nbTrials && !validPose; trial++)
{
for (int i = 0; i < 3; i++)
{
rvec.at<double>(i,0) = rng.uniform(minVal, maxVal);
tvec.at<double>(i,0) = (i == 2) ? rng.uniform(minVal*10, maxVal) : rng.uniform(-maxVal, maxVal);
}
Mat R;
cv::Rodrigues(rvec, R);
bool positiveDepth = true;
for (size_t i = 0; i < points.size() && positiveDepth; i++)
{
Matx31d objPts(points[i].x, points[i].y, points[i].z);
Mat camPts = R*objPts + tvec;
if (camPts.at<double>(2,0) <= 0)
{
positiveDepth = false;
}
}
validPose = positiveDepth;
}
}
static void generatePose(const vector<Point3f>& points, Mat& rvec, Mat& tvec, RNG& rng, int nbTrials=10)
{
vector<Point3d> points_double(points.size());
for (size_t i = 0; i < points.size(); i++)
{
points_double[i] = Point3d(points[i].x, points[i].y, points[i].z);
}
generatePose(points_double, rvec, tvec, rng, nbTrials);
}
static std::string printMethod(int method)
{
switch (method) {
case 0:
return "SOLVEPNP_ITERATIVE";
case 1:
return "SOLVEPNP_EPNP";
case 2:
return "SOLVEPNP_P3P";
case 3:
return "SOLVEPNP_DLS (remaped to SOLVEPNP_EPNP)";
case 4:
return "SOLVEPNP_UPNP (remaped to SOLVEPNP_EPNP)";
case 5:
return "SOLVEPNP_AP3P";
case 6:
return "SOLVEPNP_IPPE";
case 7:
return "SOLVEPNP_IPPE_SQUARE";
case 8:
return "SOLVEPNP_SQPNP";
default:
return "Unknown value";
}
}
class CV_solvePnPRansac_Test : public cvtest::BaseTest
{
public:
CV_solvePnPRansac_Test(bool planar_=false, bool planarTag_=false) : planar(planar_), planarTag(planarTag_)
{
eps[SOLVEPNP_ITERATIVE] = 1.0e-2;
eps[SOLVEPNP_EPNP] = 1.0e-2;
eps[SOLVEPNP_P3P] = 1.0e-2;
eps[SOLVEPNP_AP3P] = 1.0e-2;
eps[SOLVEPNP_DLS] = 1.0e-2;
eps[SOLVEPNP_UPNP] = 1.0e-2;
eps[SOLVEPNP_SQPNP] = 1.0e-2;
totalTestsCount = 10;
pointsCount = 500;
}
~CV_solvePnPRansac_Test() {}
protected:
void generate3DPointCloud(vector<Point3f>& points,
Point3f pmin = Point3f(-1, -1, 5),
Point3f pmax = Point3f(1, 1, 10))
{
RNG& rng = theRNG(); // fix the seed to use "fixed" input 3D points
for (size_t i = 0; i < points.size(); i++)
{
float _x = rng.uniform(pmin.x, pmax.x);
float _y = rng.uniform(pmin.y, pmax.y);
float _z = rng.uniform(pmin.z, pmax.z);
points[i] = Point3f(_x, _y, _z);
}
}
void generatePlanarPointCloud(vector<Point3f>& points,
Point2f pmin = Point2f(-1, -1),
Point2f pmax = Point2f(1, 1))
{
RNG& rng = theRNG(); // fix the seed to use "fixed" input 3D points
if (planarTag)
{
const float squareLength_2 = rng.uniform(0.01f, pmax.x) / 2;
points.clear();
points.push_back(Point3f(-squareLength_2, squareLength_2, 0));
points.push_back(Point3f(squareLength_2, squareLength_2, 0));
points.push_back(Point3f(squareLength_2, -squareLength_2, 0));
points.push_back(Point3f(-squareLength_2, -squareLength_2, 0));
}
else
{
Mat rvec_double, tvec_double;
generatePose(points, rvec_double, tvec_double, rng);
Mat rvec, tvec, R;
rvec_double.convertTo(rvec, CV_32F);
tvec_double.convertTo(tvec, CV_32F);
cv::Rodrigues(rvec, R);
for (size_t i = 0; i < points.size(); i++)
{
float x = rng.uniform(pmin.x, pmax.x);
float y = rng.uniform(pmin.y, pmax.y);
float z = 0;
Matx31f pt(x, y, z);
Mat pt_trans = R * pt + tvec;
points[i] = Point3f(pt_trans.at<float>(0,0), pt_trans.at<float>(1,0), pt_trans.at<float>(2,0));
}
}
}
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);
}
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, double& errorTrans, double& errorRot)
{
if ((!planar && method == SOLVEPNP_IPPE) || method == SOLVEPNP_IPPE_SQUARE)
{
return true;
}
Mat rvec, tvec;
vector<int> inliers;
Mat trueRvec, trueTvec;
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics, rng);
//UPnP is mapped to EPnP
//Uncomment this when UPnP is fixed
// if (method == SOLVEPNP_UPNP)
// {
// intrinsics.at<double>(1,1) = intrinsics.at<double>(0,0);
// }
if (mode == 0)
{
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
}
else
{
generateDistCoeffs(distCoeffs, rng);
}
generatePose(points, trueRvec, trueTvec, rng);
vector<Point2f> projectedPoints;
projectedPoints.resize(points.size());
projectPoints(points, trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
for (size_t i = 0; i < projectedPoints.size(); i++)
{
if (i % 20 == 0)
{
projectedPoints[i] = projectedPoints[rng.uniform(0,(int)points.size()-1)];
}
}
solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec, false, pointsCount, 0.5f, 0.99, inliers, method);
bool isTestSuccess = inliers.size() >= points.size()*0.95;
double rvecDiff = cvtest::norm(rvec, trueRvec, NORM_L2), tvecDiff = cvtest::norm(tvec, trueTvec, NORM_L2);
isTestSuccess = isTestSuccess && rvecDiff < eps[method] && tvecDiff < eps[method];
errorTrans = tvecDiff;
errorRot = rvecDiff;
return isTestSuccess;
}
virtual void run(int)
{
ts->set_failed_test_info(cvtest::TS::OK);
vector<Point3f> points, points_dls;
points.resize(static_cast<size_t>(pointsCount));
if (planar || planarTag)
{
generatePlanarPointCloud(points);
}
else
{
generate3DPointCloud(points);
}
RNG& rng = ts->get_rng();
for (int mode = 0; mode < 2; mode++)
{
for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++)
{
//To get the same input for each methods
RNG rngCopy = rng;
std::vector<double> vec_errorTrans, vec_errorRot;
vec_errorTrans.reserve(static_cast<size_t>(totalTestsCount));
vec_errorRot.reserve(static_cast<size_t>(totalTestsCount));
int successfulTestsCount = 0;
for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
{
double errorTrans, errorRot;
if (runTest(rngCopy, mode, method, points, errorTrans, errorRot))
{
successfulTestsCount++;
}
vec_errorTrans.push_back(errorTrans);
vec_errorRot.push_back(errorRot);
}
double maxErrorTrans = getMax(vec_errorTrans);
double maxErrorRot = getMax(vec_errorRot);
double meanErrorTrans = getMean(vec_errorTrans);
double meanErrorRot = getMean(vec_errorRot);
double medianErrorTrans = getMedian(vec_errorTrans);
double medianErrorRot = getMedian(vec_errorRot);
if (successfulTestsCount < 0.7*totalTestsCount)
{
ts->printf(cvtest::TS::LOG, "Invalid accuracy for %s, failed %d tests from %d, %s, "
"maxErrT: %f, maxErrR: %f, "
"meanErrT: %f, meanErrR: %f, "
"medErrT: %f, medErrR: %f\n",
printMethod(method).c_str(), totalTestsCount - successfulTestsCount, totalTestsCount, printMode(mode).c_str(),
maxErrorTrans, maxErrorRot, meanErrorTrans, meanErrorRot, medianErrorTrans, medianErrorRot);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
cout << "mode: " << printMode(mode) << ", method: " << printMethod(method) << " -> "
<< ((double)successfulTestsCount / totalTestsCount) * 100 << "%"
<< " (maxErrT: " << maxErrorTrans << ", maxErrR: " << maxErrorRot
<< ", meanErrT: " << meanErrorTrans << ", meanErrR: " << meanErrorRot
<< ", medErrT: " << medianErrorTrans << ", medErrR: " << medianErrorRot << ")" << endl;
double transThres, rotThresh;
findThreshold(vec_errorTrans, vec_errorRot, 0.7, transThres, rotThresh);
cout << "approximate translation threshold for 0.7: " << transThres
<< ", approximate rotation threshold for 0.7: " << rotThresh << endl;
}
cout << endl;
}
}
std::string printMode(int mode)
{
switch (mode) {
case 0:
return "no distortion";
case 1:
default:
return "distorsion";
}
}
double eps[SOLVEPNP_MAX_COUNT];
int totalTestsCount;
int pointsCount;
bool planar;
bool planarTag;
};
class CV_solvePnP_Test : public CV_solvePnPRansac_Test
{
public:
CV_solvePnP_Test(bool planar_=false, bool planarTag_=false) : CV_solvePnPRansac_Test(planar_, planarTag_)
{
eps[SOLVEPNP_ITERATIVE] = 1.0e-6;
eps[SOLVEPNP_EPNP] = 1.0e-6;
eps[SOLVEPNP_P3P] = 2.0e-4;
eps[SOLVEPNP_AP3P] = 1.0e-4;
eps[SOLVEPNP_DLS] = 1.0e-6; //DLS is remapped to EPnP, so we use the same threshold
eps[SOLVEPNP_UPNP] = 1.0e-6; //UPnP is remapped to EPnP, so we use the same threshold
eps[SOLVEPNP_IPPE] = 1.0e-6;
eps[SOLVEPNP_IPPE_SQUARE] = 1.0e-6;
eps[SOLVEPNP_SQPNP] = 1.0e-6;
totalTestsCount = 1000;
if (planar || planarTag)
{
if (planarTag)
{
pointsCount = 4;
}
else
{
pointsCount = 30;
}
}
else
{
pointsCount = 500;
}
}
~CV_solvePnP_Test() {}
protected:
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, double& errorTrans, double& errorRot)
{
if ((!planar && (method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE)) ||
(!planarTag && method == SOLVEPNP_IPPE_SQUARE))
{
errorTrans = -1;
errorRot = -1;
//SOLVEPNP_IPPE and SOLVEPNP_IPPE_SQUARE need planar object
return true;
}
//Tune thresholds...
double epsilon_trans[SOLVEPNP_MAX_COUNT];
memcpy(epsilon_trans, eps, SOLVEPNP_MAX_COUNT * sizeof(*epsilon_trans));
double epsilon_rot[SOLVEPNP_MAX_COUNT];
memcpy(epsilon_rot, eps, SOLVEPNP_MAX_COUNT * sizeof(*epsilon_rot));
if (planar)
{
if (mode == 0)
{
epsilon_trans[SOLVEPNP_EPNP] = 5.0e-3;
epsilon_trans[SOLVEPNP_DLS] = 5.0e-3;
epsilon_trans[SOLVEPNP_UPNP] = 5.0e-3;
epsilon_rot[SOLVEPNP_EPNP] = 5.0e-3;
epsilon_rot[SOLVEPNP_DLS] = 5.0e-3;
epsilon_rot[SOLVEPNP_UPNP] = 5.0e-3;
}
else
{
epsilon_trans[SOLVEPNP_ITERATIVE] = 1e-4;
epsilon_trans[SOLVEPNP_EPNP] = 5e-3;
epsilon_trans[SOLVEPNP_DLS] = 5e-3;
epsilon_trans[SOLVEPNP_UPNP] = 5e-3;
epsilon_trans[SOLVEPNP_P3P] = 1e-4;
epsilon_trans[SOLVEPNP_AP3P] = 1e-4;
epsilon_trans[SOLVEPNP_IPPE] = 1e-4;
epsilon_trans[SOLVEPNP_IPPE_SQUARE] = 1e-4;
epsilon_rot[SOLVEPNP_ITERATIVE] = 1e-4;
epsilon_rot[SOLVEPNP_EPNP] = 5e-3;
epsilon_rot[SOLVEPNP_DLS] = 5e-3;
epsilon_rot[SOLVEPNP_UPNP] = 5e-3;
epsilon_rot[SOLVEPNP_P3P] = 1e-4;
epsilon_rot[SOLVEPNP_AP3P] = 1e-4;
epsilon_rot[SOLVEPNP_IPPE] = 1e-4;
epsilon_rot[SOLVEPNP_IPPE_SQUARE] = 1e-4;
}
}
Mat trueRvec, trueTvec;
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics, rng);
//UPnP is mapped to EPnP
//Uncomment this when UPnP is fixed
// if (method == SOLVEPNP_UPNP)
// {
// intrinsics.at<double>(1,1) = intrinsics.at<double>(0,0);
// }
if (mode == 0)
{
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
}
else
{
generateDistCoeffs(distCoeffs, rng);
}
generatePose(points, trueRvec, trueTvec, rng);
std::vector<Point3f> opoints;
switch(method)
{
case SOLVEPNP_P3P:
case SOLVEPNP_AP3P:
opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
break;
//UPnP is mapped to EPnP
//Uncomment this when UPnP is fixed
// case SOLVEPNP_UPNP:
// if (points.size() > 50)
// {
// opoints = std::vector<Point3f>(points.begin(), points.begin()+50);
// }
// else
// {
// opoints = points;
// }
// break;
default:
opoints = points;
break;
}
vector<Point2f> projectedPoints;
projectedPoints.resize(opoints.size());
projectPoints(opoints, trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
Mat rvec, tvec;
bool isEstimateSuccess = solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec, false, method);
if (!isEstimateSuccess)
{
return false;
}
double rvecDiff = cvtest::norm(rvec, trueRvec, NORM_L2), tvecDiff = cvtest::norm(tvec, trueTvec, NORM_L2);
bool isTestSuccess = rvecDiff < epsilon_rot[method] && tvecDiff < epsilon_trans[method];
errorTrans = tvecDiff;
errorRot = rvecDiff;
return isTestSuccess;
}
};
class CV_solveP3P_Test : public CV_solvePnPRansac_Test
{
public:
CV_solveP3P_Test()
{
eps[SOLVEPNP_P3P] = 2.0e-4;
eps[SOLVEPNP_AP3P] = 1.0e-4;
totalTestsCount = 1000;
}
~CV_solveP3P_Test() {}
protected:
virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, double& errorTrans, double& errorRot)
{
std::vector<Mat> rvecs, tvecs;
Mat trueRvec, trueTvec;
Mat intrinsics, distCoeffs;
generateCameraMatrix(intrinsics, rng);
if (mode == 0)
{
distCoeffs = Mat::zeros(4, 1, CV_64FC1);
}
else
{
generateDistCoeffs(distCoeffs, rng);
}
generatePose(points, trueRvec, trueTvec, rng);
std::vector<Point3f> opoints;
opoints = std::vector<Point3f>(points.begin(), points.begin()+3);
vector<Point2f> projectedPoints;
projectedPoints.resize(opoints.size());
projectPoints(opoints, trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
int num_of_solutions = solveP3P(opoints, projectedPoints, intrinsics, distCoeffs, rvecs, tvecs, method);
if (num_of_solutions != (int) rvecs.size() || num_of_solutions != (int) tvecs.size() || num_of_solutions == 0)
{
return false;
}
bool isTestSuccess = false;
for (size_t i = 0; i < rvecs.size() && !isTestSuccess; i++) {
double rvecDiff = cvtest::norm(rvecs[i], trueRvec, NORM_L2);
double tvecDiff = cvtest::norm(tvecs[i], trueTvec, NORM_L2);
isTestSuccess = rvecDiff < eps[method] && tvecDiff < eps[method];
errorTrans = std::min(errorTrans, tvecDiff);
errorRot = std::min(errorRot, rvecDiff);
}
return isTestSuccess;
}
virtual void run(int)
{
ts->set_failed_test_info(cvtest::TS::OK);
vector<Point3f> points;
points.resize(static_cast<size_t>(pointsCount));
generate3DPointCloud(points);
const int methodsCount = 2;
int methods[] = {SOLVEPNP_P3P, SOLVEPNP_AP3P};
RNG rng = ts->get_rng();
for (int mode = 0; mode < 2; mode++)
{
//To get the same input for each methods
RNG rngCopy = rng;
for (int method = 0; method < methodsCount; method++)
{
std::vector<double> vec_errorTrans, vec_errorRot;
vec_errorTrans.reserve(static_cast<size_t>(totalTestsCount));
vec_errorRot.reserve(static_cast<size_t>(totalTestsCount));
int successfulTestsCount = 0;
for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
{
double errorTrans = 0, errorRot = 0;
if (runTest(rngCopy, mode, methods[method], points, errorTrans, errorRot))
{
successfulTestsCount++;
}
vec_errorTrans.push_back(errorTrans);
vec_errorRot.push_back(errorRot);
}
double maxErrorTrans = getMax(vec_errorTrans);
double maxErrorRot = getMax(vec_errorRot);
double meanErrorTrans = getMean(vec_errorTrans);
double meanErrorRot = getMean(vec_errorRot);
double medianErrorTrans = getMedian(vec_errorTrans);
double medianErrorRot = getMedian(vec_errorRot);
if (successfulTestsCount < 0.7*totalTestsCount)
{
ts->printf(cvtest::TS::LOG, "Invalid accuracy for %s, failed %d tests from %d, %s, "
"maxErrT: %f, maxErrR: %f, "
"meanErrT: %f, meanErrR: %f, "
"medErrT: %f, medErrR: %f\n",
printMethod(methods[method]).c_str(), totalTestsCount - successfulTestsCount, totalTestsCount, printMode(mode).c_str(),
maxErrorTrans, maxErrorRot, meanErrorTrans, meanErrorRot, medianErrorTrans, medianErrorRot);
ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
}
cout << "mode: " << printMode(mode) << ", method: " << printMethod(methods[method]) << " -> "
<< ((double)successfulTestsCount / totalTestsCount) * 100 << "%"
<< " (maxErrT: " << maxErrorTrans << ", maxErrR: " << maxErrorRot
<< ", meanErrT: " << meanErrorTrans << ", meanErrR: " << meanErrorRot
<< ", medErrT: " << medianErrorTrans << ", medErrR: " << medianErrorRot << ")" << endl;
double transThres, rotThresh;
findThreshold(vec_errorTrans, vec_errorRot, 0.7, transThres, rotThresh);
cout << "approximate translation threshold for 0.7: " << transThres
<< ", approximate rotation threshold for 0.7: " << rotThresh << endl;
}
}
}
};
TEST(Calib3d_SolveP3P, accuracy) { CV_solveP3P_Test test; test.safe_run();}
TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }
TEST(Calib3d_SolvePnP, accuracy_planar) { CV_solvePnP_Test test(true); test.safe_run(); }
TEST(Calib3d_SolvePnP, accuracy_planar_tag) { CV_solvePnP_Test test(true, true); test.safe_run(); }
TEST(Calib3d_SolvePnPRansac, concurrency)
{
int count = 7*13;
Mat object(1, count, CV_32FC3);
randu(object, -100, 100);
Mat camera_mat(3, 3, CV_32FC1);
randu(camera_mat, 0.5, 1);
camera_mat.at<float>(0, 1) = 0.f;
camera_mat.at<float>(1, 0) = 0.f;
camera_mat.at<float>(2, 0) = 0.f;
camera_mat.at<float>(2, 1) = 0.f;
camera_mat.at<float>(2, 2) = 1.f;
Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0));
vector<cv::Point2f> image_vec;
Mat rvec_gold(1, 3, CV_32FC1);
randu(rvec_gold, 0, 1);
Mat tvec_gold(1, 3, CV_32FC1);
randu(tvec_gold, 0, 1);
projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec);
Mat image(1, count, CV_32FC2, &image_vec[0]);
Mat rvec1, rvec2;
Mat tvec1, tvec2;
int threads = getNumThreads();
{
// limit concurrency to get deterministic result
theRNG().state = 20121010;
setNumThreads(1);
solvePnPRansac(object, image, camera_mat, dist_coef, rvec1, tvec1);
}
{
setNumThreads(threads);
Mat rvec;
Mat tvec;
// parallel executions
for(int i = 0; i < 10; ++i)
{
cv::theRNG().state = 20121010;
solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
}
}
{
// single thread again
theRNG().state = 20121010;
setNumThreads(1);
solvePnPRansac(object, image, camera_mat, dist_coef, rvec2, tvec2);
}
double rnorm = cvtest::norm(rvec1, rvec2, NORM_INF);
double tnorm = cvtest::norm(tvec1, tvec2, NORM_INF);
EXPECT_LT(rnorm, 1e-6);
EXPECT_LT(tnorm, 1e-6);
}
TEST(Calib3d_SolvePnPRansac, input_type)
{
const int numPoints = 10;
Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0.,
5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.);
std::vector<cv::Point3f> points3d;
std::vector<cv::Point2f> points2d;
for (int i = 0; i < numPoints; i+=2)
{
points3d.push_back(cv::Point3i(5+i, 3, 2));
points3d.push_back(cv::Point3i(5+i, 3+i, 2+i));
points2d.push_back(cv::Point2i(0, i));
points2d.push_back(cv::Point2i(-i, i));
}
Mat R1, t1, R2, t2, R3, t3, R4, t4;
EXPECT_TRUE(solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R1, t1));
Mat points3dMat(points3d);
Mat points2dMat(points2d);
EXPECT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R2, t2));
points3dMat = points3dMat.reshape(3, 1);
points2dMat = points2dMat.reshape(2, 1);
EXPECT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R3, t3));
points3dMat = points3dMat.reshape(1, numPoints);
points2dMat = points2dMat.reshape(1, numPoints);
EXPECT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R4, t4));
EXPECT_LE(cvtest::norm(R1, R2, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(t1, t2, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(R1, R3, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(t1, t3, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(R1, R4, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(t1, t4, NORM_INF), 1e-6);
}
TEST(Calib3d_SolvePnPRansac, double_support)
{
Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0.,
5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.);
std::vector<cv::Point3d> points3d;
std::vector<cv::Point2d> points2d;
std::vector<cv::Point3f> points3dF;
std::vector<cv::Point2f> points2dF;
for (int i = 0; i < 10 ; i+=2)
{
points3d.push_back(cv::Point3d(5+i, 3, 2));
points3dF.push_back(cv::Point3f(static_cast<float>(5+i), 3, 2));
points3d.push_back(cv::Point3d(5+i, 3+i, 2+i));
points3dF.push_back(cv::Point3f(static_cast<float>(5+i), static_cast<float>(3+i), static_cast<float>(2+i)));
points2d.push_back(cv::Point2d(0, i));
points2dF.push_back(cv::Point2f(0, static_cast<float>(i)));
points2d.push_back(cv::Point2d(-i, i));
points2dF.push_back(cv::Point2f(static_cast<float>(-i), static_cast<float>(i)));
}
Mat R, t, RF, tF;
vector<int> inliers;
solvePnPRansac(points3dF, points2dF, intrinsics, cv::Mat(), RF, tF, true, 100, 8.f, 0.999, inliers, cv::SOLVEPNP_P3P);
solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R, t, true, 100, 8.f, 0.999, inliers, cv::SOLVEPNP_P3P);
EXPECT_LE(cvtest::norm(R, Mat_<double>(RF), NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t, Mat_<double>(tF), NORM_INF), 1e-3);
}
TEST(Calib3d_SolvePnPRansac, bad_input_points_19253)
{
// with this specific data
// when computing the final pose using points in the consensus set with SOLVEPNP_ITERATIVE and solvePnP()
// an exception is thrown from solvePnP because there are 5 non-coplanar 3D points and the DLT algorithm needs at least 6 non-coplanar 3D points
// with PR #19253 we choose to return true, with the pose estimated from the MSS stage instead of throwing the exception
float pts2d_[] = {
-5.38358629e-01f, -5.09638414e-02f,
-5.07192254e-01f, -2.20743284e-01f,
-5.43107152e-01f, -4.90474701e-02f,
-5.54325163e-01f, -1.86715424e-01f,
-5.59334219e-01f, -4.01909500e-02f,
-5.43504596e-01f, -4.61776406e-02f
};
Mat pts2d(6, 2, CV_32FC1, pts2d_);
float pts3d_[] = {
-3.01153604e-02f, -1.55665115e-01f, 4.50000018e-01f,
4.27827090e-01f, 4.28645730e-01f, 1.08600008e+00f,
-3.14165242e-02f, -1.52656138e-01f, 4.50000018e-01f,
-1.46217480e-01f, 5.57961613e-02f, 7.17000008e-01f,
-4.89348806e-02f, -1.38795510e-01f, 4.47000027e-01f,
-3.13065052e-02f, -1.52636901e-01f, 4.51000035e-01f
};
Mat pts3d(6, 3, CV_32FC1, pts3d_);
Mat camera_mat = Mat::eye(3, 3, CV_64FC1);
Mat rvec, tvec;
vector<int> inliers;
// solvePnPRansac will return true with 5 inliers, which means the result is from MSS stage.
bool result = solvePnPRansac(pts3d, pts2d, camera_mat, noArray(), rvec, tvec, false, 100, 4.f / 460.f, 0.99, inliers);
EXPECT_EQ(inliers.size(), size_t(5));
EXPECT_TRUE(result);
}
TEST(Calib3d_SolvePnP, input_type)
{
Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0.,
5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.);
vector<Point3d> points3d_;
vector<Point3f> points3dF_;
//Cube
const float l = -0.1f;
//Front face
points3d_.push_back(Point3d(-l, -l, -l));
points3dF_.push_back(Point3f(-l, -l, -l));
points3d_.push_back(Point3d(l, -l, -l));
points3dF_.push_back(Point3f(l, -l, -l));
points3d_.push_back(Point3d(l, l, -l));
points3dF_.push_back(Point3f(l, l, -l));
points3d_.push_back(Point3d(-l, l, -l));
points3dF_.push_back(Point3f(-l, l, -l));
//Back face
points3d_.push_back(Point3d(-l, -l, l));
points3dF_.push_back(Point3f(-l, -l, l));
points3d_.push_back(Point3d(l, -l, l));
points3dF_.push_back(Point3f(l, -l, l));
points3d_.push_back(Point3d(l, l, l));
points3dF_.push_back(Point3f(l, l, l));
points3d_.push_back(Point3d(-l, l, l));
points3dF_.push_back(Point3f(-l, l, l));
Mat trueRvec = (Mat_<double>(3,1) << 0.1, -0.25, 0.467);
Mat trueTvec = (Mat_<double>(3,1) << -0.21, 0.12, 0.746);
for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++)
{
vector<Point3d> points3d;
vector<Point2d> points2d;
vector<Point3f> points3dF;
vector<Point2f> points2dF;
if (method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE)
{
const float tagSize_2 = 0.05f / 2;
points3d.push_back(Point3d(-tagSize_2, tagSize_2, 0));
points3d.push_back(Point3d( tagSize_2, tagSize_2, 0));
points3d.push_back(Point3d( tagSize_2, -tagSize_2, 0));
points3d.push_back(Point3d(-tagSize_2, -tagSize_2, 0));
points3dF.push_back(Point3f(-tagSize_2, tagSize_2, 0));
points3dF.push_back(Point3f( tagSize_2, tagSize_2, 0));
points3dF.push_back(Point3f( tagSize_2, -tagSize_2, 0));
points3dF.push_back(Point3f(-tagSize_2, -tagSize_2, 0));
}
else if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P)
{
points3d = vector<Point3d>(points3d_.begin(), points3d_.begin()+4);
points3dF = vector<Point3f>(points3dF_.begin(), points3dF_.begin()+4);
}
else
{
points3d = points3d_;
points3dF = points3dF_;
}
projectPoints(points3d, trueRvec, trueTvec, intrinsics, noArray(), points2d);
projectPoints(points3dF, trueRvec, trueTvec, intrinsics, noArray(), points2dF);
//solvePnP
{
Mat R, t, RF, tF;
solvePnP(points3dF, points2dF, Matx33f(intrinsics), Mat(), RF, tF, false, method);
solvePnP(points3d, points2d, intrinsics, Mat(), R, t, false, method);
//By default rvec and tvec must be returned in double precision
EXPECT_EQ(RF.type(), tF.type());
EXPECT_EQ(RF.type(), CV_64FC1);
EXPECT_EQ(R.type(), t.type());
EXPECT_EQ(R.type(), CV_64FC1);
EXPECT_LE(cvtest::norm(R, RF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t, tF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, RF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, tF, NORM_INF), 1e-3);
}
{
Mat R1, t1, R2, t2;
solvePnP(points3dF, points2d, intrinsics, Mat(), R1, t1, false, method);
solvePnP(points3d, points2dF, intrinsics, Mat(), R2, t2, false, method);
//By default rvec and tvec must be returned in double precision
EXPECT_EQ(R1.type(), t1.type());
EXPECT_EQ(R1.type(), CV_64FC1);
EXPECT_EQ(R2.type(), t2.type());
EXPECT_EQ(R2.type(), CV_64FC1);
EXPECT_LE(cvtest::norm(R1, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, t2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t2, NORM_INF), 1e-3);
}
{
Mat R1(3,1,CV_32FC1), t1(3,1,CV_64FC1);
Mat R2(3,1,CV_64FC1), t2(3,1,CV_32FC1);
solvePnP(points3dF, points2d, intrinsics, Mat(), R1, t1, false, method);
solvePnP(points3d, points2dF, intrinsics, Mat(), R2, t2, false, method);
//If not null, rvec and tvec must be returned in the same precision
EXPECT_EQ(R1.type(), CV_32FC1);
EXPECT_EQ(t1.type(), CV_64FC1);
EXPECT_EQ(R2.type(), CV_64FC1);
EXPECT_EQ(t2.type(), CV_32FC1);
EXPECT_LE(cvtest::norm(Mat_<double>(R1), R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, Mat_<double>(t2), NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, Mat_<double>(R1), NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, Mat_<double>(t2), NORM_INF), 1e-3);
}
{
Matx31f R1, t2;
Matx31d R2, t1;
solvePnP(points3dF, points2d, intrinsics, Mat(), R1, t1, false, method);
solvePnP(points3d, points2dF, intrinsics, Mat(), R2, t2, false, method);
Matx31d R1d(R1(0), R1(1), R1(2));
Matx31d t2d(t2(0), t2(1), t2(2));
EXPECT_LE(cvtest::norm(R1d, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, t2d, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R1d, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t2d, NORM_INF), 1e-3);
}
//solvePnPGeneric
{
vector<Mat> Rs, ts, RFs, tFs;
int res1 = solvePnPGeneric(points3dF, points2dF, Matx33f(intrinsics), Mat(), RFs, tFs, false, (SolvePnPMethod)method);
int res2 = solvePnPGeneric(points3d, points2d, intrinsics, Mat(), Rs, ts, false, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Mat R = Rs.front(), t = ts.front(), RF = RFs.front(), tF = tFs.front();
//By default rvecs and tvecs must be returned in double precision
EXPECT_EQ(RF.type(), tF.type());
EXPECT_EQ(RF.type(), CV_64FC1);
EXPECT_EQ(R.type(), t.type());
EXPECT_EQ(R.type(), CV_64FC1);
EXPECT_LE(cvtest::norm(R, RF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t, tF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, RF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, tF, NORM_INF), 1e-3);
}
{
vector<Mat> R1s, t1s, R2s, t2s;
int res1 = solvePnPGeneric(points3dF, points2d, intrinsics, Mat(), R1s, t1s, false, (SolvePnPMethod)method);
int res2 = solvePnPGeneric(points3d, points2dF, intrinsics, Mat(), R2s, t2s, false, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Mat R1 = R1s.front(), t1 = t1s.front(), R2 = R2s.front(), t2 = t2s.front();
//By default rvecs and tvecs must be returned in double precision
EXPECT_EQ(R1.type(), t1.type());
EXPECT_EQ(R1.type(), CV_64FC1);
EXPECT_EQ(R2.type(), t2.type());
EXPECT_EQ(R2.type(), CV_64FC1);
EXPECT_LE(cvtest::norm(R1, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, t2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t2, NORM_INF), 1e-3);
}
{
vector<Mat_<float> > R1s, t2s;
vector<Mat_<double> > R2s, t1s;
int res1 = solvePnPGeneric(points3dF, points2d, intrinsics, Mat(), R1s, t1s, false, (SolvePnPMethod)method);
int res2 = solvePnPGeneric(points3d, points2dF, intrinsics, Mat(), R2s, t2s, false, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Mat R1 = R1s.front(), t1 = t1s.front();
Mat R2 = R2s.front(), t2 = t2s.front();
//If not null, rvecs and tvecs must be returned in the same precision
EXPECT_EQ(R1.type(), CV_32FC1);
EXPECT_EQ(t1.type(), CV_64FC1);
EXPECT_EQ(R2.type(), CV_64FC1);
EXPECT_EQ(t2.type(), CV_32FC1);
EXPECT_LE(cvtest::norm(Mat_<double>(R1), R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, Mat_<double>(t2), NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, Mat_<double>(R1), NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, Mat_<double>(t2), NORM_INF), 1e-3);
}
{
vector<Matx31f> R1s, t2s;
vector<Matx31d> R2s, t1s;
int res1 = solvePnPGeneric(points3dF, points2d, intrinsics, Mat(), R1s, t1s, false, (SolvePnPMethod)method);
int res2 = solvePnPGeneric(points3d, points2dF, intrinsics, Mat(), R2s, t2s, false, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Matx31f R1 = R1s.front(), t2 = t2s.front();
Matx31d R2 = R2s.front(), t1 = t1s.front();
Matx31d R1d(R1(0), R1(1), R1(2)), t2d(t2(0), t2(1), t2(2));
EXPECT_LE(cvtest::norm(R1d, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, t2d, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R1d, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t2d, NORM_INF), 1e-3);
}
if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P)
{
//solveP3P
{
vector<Mat> Rs, ts, RFs, tFs;
int res1 = solveP3P(points3dF, points2dF, Matx33f(intrinsics), Mat(), RFs, tFs, (SolvePnPMethod)method);
int res2 = solveP3P(points3d, points2d, intrinsics, Mat(), Rs, ts, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Mat R = Rs.front(), t = ts.front(), RF = RFs.front(), tF = tFs.front();
//By default rvecs and tvecs must be returned in double precision
EXPECT_EQ(RF.type(), tF.type());
EXPECT_EQ(RF.type(), CV_64FC1);
EXPECT_EQ(R.type(), t.type());
EXPECT_EQ(R.type(), CV_64FC1);
EXPECT_LE(cvtest::norm(R, RF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t, tF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, RF, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, tF, NORM_INF), 1e-3);
}
{
vector<Mat> R1s, t1s, R2s, t2s;
int res1 = solveP3P(points3dF, points2d, intrinsics, Mat(), R1s, t1s, (SolvePnPMethod)method);
int res2 = solveP3P(points3d, points2dF, intrinsics, Mat(), R2s, t2s, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Mat R1 = R1s.front(), t1 = t1s.front(), R2 = R2s.front(), t2 = t2s.front();
//By default rvecs and tvecs must be returned in double precision
EXPECT_EQ(R1.type(), t1.type());
EXPECT_EQ(R1.type(), CV_64FC1);
EXPECT_EQ(R2.type(), t2.type());
EXPECT_EQ(R2.type(), CV_64FC1);
EXPECT_LE(cvtest::norm(R1, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, t2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t2, NORM_INF), 1e-3);
}
{
vector<Mat_<float> > R1s, t2s;
vector<Mat_<double> > R2s, t1s;
int res1 = solveP3P(points3dF, points2d, intrinsics, Mat(), R1s, t1s, (SolvePnPMethod)method);
int res2 = solveP3P(points3d, points2dF, intrinsics, Mat(), R2s, t2s, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Mat R1 = R1s.front(), t1 = t1s.front();
Mat R2 = R2s.front(), t2 = t2s.front();
//If not null, rvecs and tvecs must be returned in the same precision
EXPECT_EQ(R1.type(), CV_32FC1);
EXPECT_EQ(t1.type(), CV_64FC1);
EXPECT_EQ(R2.type(), CV_64FC1);
EXPECT_EQ(t2.type(), CV_32FC1);
EXPECT_LE(cvtest::norm(Mat_<double>(R1), R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, Mat_<double>(t2), NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, Mat_<double>(R1), NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, Mat_<double>(t2), NORM_INF), 1e-3);
}
{
vector<Matx31f> R1s, t2s;
vector<Matx31d> R2s, t1s;
int res1 = solveP3P(points3dF, points2d, intrinsics, Mat(), R1s, t1s, (SolvePnPMethod)method);
int res2 = solveP3P(points3d, points2dF, intrinsics, Mat(), R2s, t2s, (SolvePnPMethod)method);
EXPECT_GT(res1, 0);
EXPECT_GT(res2, 0);
Matx31f R1 = R1s.front(), t2 = t2s.front();
Matx31d R2 = R2s.front(), t1 = t1s.front();
Matx31d R1d(R1(0), R1(1), R1(2)), t2d(t2(0), t2(1), t2(2));
EXPECT_LE(cvtest::norm(R1d, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(t1, t2d, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R1d, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t1, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueRvec, R2, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(trueTvec, t2d, NORM_INF), 1e-3);
}
}
}
}
TEST(Calib3d_SolvePnP, translation)
{
Mat cameraIntrinsic = Mat::eye(3,3, CV_32FC1);
vector<float> crvec;
crvec.push_back(0.f);
crvec.push_back(0.f);
crvec.push_back(0.f);
vector<float> ctvec;
ctvec.push_back(100.f);
ctvec.push_back(100.f);
ctvec.push_back(0.f);
vector<Point3f> p3d;
p3d.push_back(Point3f(0,0,0));
p3d.push_back(Point3f(0,0,10));
p3d.push_back(Point3f(0,10,10));
p3d.push_back(Point3f(10,10,10));
p3d.push_back(Point3f(2,5,5));
p3d.push_back(Point3f(-4,8,6));
vector<Point2f> p2d;
projectPoints(p3d, crvec, ctvec, cameraIntrinsic, noArray(), p2d);
Mat rvec;
Mat tvec;
rvec =(Mat_<float>(3,1) << 0, 0, 0);
tvec = (Mat_<float>(3,1) << 100, 100, 0);
solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true);
EXPECT_TRUE(checkRange(rvec));
EXPECT_TRUE(checkRange(tvec));
rvec =(Mat_<double>(3,1) << 0, 0, 0);
tvec = (Mat_<double>(3,1) << 100, 100, 0);
solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true);
EXPECT_TRUE(checkRange(rvec));
EXPECT_TRUE(checkRange(tvec));
solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, false);
EXPECT_TRUE(checkRange(rvec));
EXPECT_TRUE(checkRange(tvec));
}
TEST(Calib3d_SolvePnP, iterativeInitialGuess3pts)
{
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
Mat rvec_est = (Mat_<double>(3,1) << 0.2, -0.1, 0.6);
Mat tvec_est = (Mat_<double>(3,1) << 0.05, -0.05, 1.0);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
EXPECT_EQ(rvec_est.type(), CV_64FC1);
EXPECT_EQ(tvec_est.type(), CV_64FC1);
}
{
Matx33f intrinsics(605.4f, 0.0f, 317.35f,
0.0f, 601.2f, 242.63f,
0.0f, 0.0f, 1.0f);
float L = 0.1f;
vector<Point3f> p3d;
p3d.push_back(Point3f(-L, -L, 0.0f));
p3d.push_back(Point3f(L, -L, 0.0f));
p3d.push_back(Point3f(L, L, 0.0f));
Mat rvec_ground_truth = (Mat_<float>(3,1) << -0.75f, 0.4f, 0.34f);
Mat tvec_ground_truth = (Mat_<float>(3,1) << -0.15f, 0.35f, 1.58f);
vector<Point2f> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
Mat rvec_est = (Mat_<float>(3,1) << -0.5f, 0.2f, 0.2f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.2f, 1.0f);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
EXPECT_EQ(rvec_est.type(), CV_32FC1);
EXPECT_EQ(tvec_est.type(), CV_32FC1);
}
}
TEST(Calib3d_SolvePnP, iterativeInitialGuess)
{
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
p3d.push_back(Point3d(-L, L, L/2));
p3d.push_back(Point3d(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
Mat rvec_est = (Mat_<double>(3,1) << 0.1, -0.1, 0.1);
Mat tvec_est = (Mat_<double>(3,1) << 0.0, -0.5, 1.0);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
EXPECT_EQ(rvec_est.type(), CV_64FC1);
EXPECT_EQ(tvec_est.type(), CV_64FC1);
}
{
Matx33f intrinsics(605.4f, 0.0f, 317.35f,
0.0f, 601.2f, 242.63f,
0.0f, 0.0f, 1.0f);
float L = 0.1f;
vector<Point3f> p3d;
p3d.push_back(Point3f(-L, -L, 0.0f));
p3d.push_back(Point3f(L, -L, 0.0f));
p3d.push_back(Point3f(L, L, 0.0f));
p3d.push_back(Point3f(-L, L, L/2));
p3d.push_back(Point3f(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<float>(3,1) << -0.75f, 0.4f, 0.34f);
Mat tvec_ground_truth = (Mat_<float>(3,1) << -0.15f, 0.35f, 1.58f);
vector<Point2f> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
Mat rvec_est = (Mat_<float>(3,1) << -0.1f, 0.1f, 0.1f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.0f, 1.0f);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
EXPECT_EQ(rvec_est.type(), CV_32FC1);
EXPECT_EQ(tvec_est.type(), CV_32FC1);
}
}
TEST(Calib3d_SolvePnP, generic)
{
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d_;
p3d_.push_back(Point3d(-L, L, 0));
p3d_.push_back(Point3d(L, L, 0));
p3d_.push_back(Point3d(L, -L, 0));
p3d_.push_back(Point3d(-L, -L, 0));
p3d_.push_back(Point3d(-L, L, L/2));
p3d_.push_back(Point3d(0, 0, -L/2));
const int ntests = 10;
for (int numTest = 0; numTest < ntests; numTest++)
{
Mat rvec_ground_truth;
Mat tvec_ground_truth;
generatePose(p3d_, rvec_ground_truth, tvec_ground_truth, theRNG());
vector<Point2d> p2d_;
projectPoints(p3d_, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d_);
for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++)
{
vector<Mat> rvecs_est;
vector<Mat> tvecs_est;
vector<Point3d> p3d;
vector<Point2d> p2d;
if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P ||
method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE)
{
p3d = vector<Point3d>(p3d_.begin(), p3d_.begin()+4);
p2d = vector<Point2d>(p2d_.begin(), p2d_.begin()+4);
}
else
{
p3d = p3d_;
p2d = p2d_;
}
vector<double> reprojectionErrors;
solvePnPGeneric(p3d, p2d, intrinsics, noArray(), rvecs_est, tvecs_est, false, (SolvePnPMethod)method,
noArray(), noArray(), reprojectionErrors);
EXPECT_TRUE(!rvecs_est.empty());
EXPECT_TRUE(rvecs_est.size() == tvecs_est.size() && tvecs_est.size() == reprojectionErrors.size());
for (size_t i = 0; i < reprojectionErrors.size()-1; i++)
{
EXPECT_GE(reprojectionErrors[i+1], reprojectionErrors[i]);
}
bool isTestSuccess = false;
for (size_t i = 0; i < rvecs_est.size() && !isTestSuccess; i++) {
double rvecDiff = cvtest::norm(rvecs_est[i], rvec_ground_truth, NORM_L2);
double tvecDiff = cvtest::norm(tvecs_est[i], tvec_ground_truth, NORM_L2);
const double threshold = method == SOLVEPNP_P3P ? 1e-2 : 1e-4;
isTestSuccess = rvecDiff < threshold && tvecDiff < threshold;
}
EXPECT_TRUE(isTestSuccess);
}
}
}
{
Matx33f intrinsics(605.4f, 0.0f, 317.35f,
0.0f, 601.2f, 242.63f,
0.0f, 0.0f, 1.0f);
float L = 0.1f;
vector<Point3f> p3f_;
p3f_.push_back(Point3f(-L, L, 0));
p3f_.push_back(Point3f(L, L, 0));
p3f_.push_back(Point3f(L, -L, 0));
p3f_.push_back(Point3f(-L, -L, 0));
p3f_.push_back(Point3f(-L, L, L/2));
p3f_.push_back(Point3f(0, 0, -L/2));
const int ntests = 10;
for (int numTest = 0; numTest < ntests; numTest++)
{
Mat rvec_ground_truth;
Mat tvec_ground_truth;
generatePose(p3f_, rvec_ground_truth, tvec_ground_truth, theRNG());
vector<Point2f> p2f_;
projectPoints(p3f_, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2f_);
for (int method = 0; method < SOLVEPNP_MAX_COUNT; method++)
{
vector<Mat> rvecs_est;
vector<Mat> tvecs_est;
vector<Point3f> p3f;
vector<Point2f> p2f;
if (method == SOLVEPNP_P3P || method == SOLVEPNP_AP3P ||
method == SOLVEPNP_IPPE || method == SOLVEPNP_IPPE_SQUARE)
{
p3f = vector<Point3f>(p3f_.begin(), p3f_.begin()+4);
p2f = vector<Point2f>(p2f_.begin(), p2f_.begin()+4);
}
else
{
p3f = p3f_;
p2f = p2f_;
}
vector<double> reprojectionErrors;
solvePnPGeneric(p3f, p2f, intrinsics, noArray(), rvecs_est, tvecs_est, false, (SolvePnPMethod)method,
noArray(), noArray(), reprojectionErrors);
EXPECT_TRUE(!rvecs_est.empty());
EXPECT_TRUE(rvecs_est.size() == tvecs_est.size() && tvecs_est.size() == reprojectionErrors.size());
for (size_t i = 0; i < reprojectionErrors.size()-1; i++)
{
EXPECT_GE(reprojectionErrors[i+1], reprojectionErrors[i]);
}
bool isTestSuccess = false;
for (size_t i = 0; i < rvecs_est.size() && !isTestSuccess; i++) {
double rvecDiff = cvtest::norm(rvecs_est[i], rvec_ground_truth, NORM_L2);
double tvecDiff = cvtest::norm(tvecs_est[i], tvec_ground_truth, NORM_L2);
const double threshold = method == SOLVEPNP_P3P ? 1e-2 : 1e-4;
isTestSuccess = rvecDiff < threshold && tvecDiff < threshold;
}
EXPECT_TRUE(isTestSuccess);
}
}
}
}
TEST(Calib3d_SolvePnP, refine3pts)
{
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<double>(3,1) << 0.2, -0.1, 0.6);
Mat tvec_est = (Mat_<double>(3,1) << 0.05, -0.05, 1.0);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<double>(3,1) << 0.2, -0.1, 0.6);
Mat tvec_est = (Mat_<double>(3,1) << 0.05, -0.05, 1.0);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
{
Matx33f intrinsics(605.4f, 0.0f, 317.35f,
0.0f, 601.2f, 242.63f,
0.0f, 0.0f, 1.0f);
float L = 0.1f;
vector<Point3f> p3d;
p3d.push_back(Point3f(-L, -L, 0.0f));
p3d.push_back(Point3f(L, -L, 0.0f));
p3d.push_back(Point3f(L, L, 0.0f));
Mat rvec_ground_truth = (Mat_<float>(3,1) << -0.75f, 0.4f, 0.34f);
Mat tvec_ground_truth = (Mat_<float>(3,1) << -0.15f, 0.35f, 1.58f);
vector<Point2f> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<float>(3,1) << -0.5f, 0.2f, 0.2f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.2f, 1.0f);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<float>(3,1) << -0.5f, 0.2f, 0.2f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.2f, 1.0f);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
}
TEST(Calib3d_SolvePnP, refine)
{
//double
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
p3d.push_back(Point3d(-L, L, L/2));
p3d.push_back(Point3d(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<double>(3,1) << 0.1, -0.1, 0.1);
Mat tvec_est = (Mat_<double>(3,1) << 0.0, -0.5, 1.0);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "\nmethod: Levenberg-Marquardt (C API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<double>(3,1) << 0.1, -0.1, 0.1);
Mat tvec_est = (Mat_<double>(3,1) << 0.0, -0.5, 1.0);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<double>(3,1) << 0.1, -0.1, 0.1);
Mat tvec_est = (Mat_<double>(3,1) << 0.0, -0.5, 1.0);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
//float
{
Matx33f intrinsics(605.4f, 0.0f, 317.35f,
0.0f, 601.2f, 242.63f,
0.0f, 0.0f, 1.0f);
float L = 0.1f;
vector<Point3f> p3d;
p3d.push_back(Point3f(-L, -L, 0.0f));
p3d.push_back(Point3f(L, -L, 0.0f));
p3d.push_back(Point3f(L, L, 0.0f));
p3d.push_back(Point3f(-L, L, L/2));
p3d.push_back(Point3f(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<float>(3,1) << -0.75f, 0.4f, 0.34f);
Mat tvec_ground_truth = (Mat_<float>(3,1) << -0.15f, 0.35f, 1.58f);
vector<Point2f> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
{
Mat rvec_est = (Mat_<float>(3,1) << -0.1f, 0.1f, 0.1f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.0f, 1.0f);
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, true, SOLVEPNP_ITERATIVE);
cout << "\nmethod: Levenberg-Marquardt (C API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<float>(3,1) << -0.1f, 0.1f, 0.1f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.0f, 1.0f);
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
{
Mat rvec_est = (Mat_<float>(3,1) << -0.1f, 0.1f, 0.1f);
Mat tvec_est = (Mat_<float>(3,1) << 0.0f, 0.0f, 1.0f);
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-6);
}
}
//refine after solvePnP
{
Matx33d intrinsics(605.4, 0.0, 317.35,
0.0, 601.2, 242.63,
0.0, 0.0, 1.0);
double L = 0.1;
vector<Point3d> p3d;
p3d.push_back(Point3d(-L, -L, 0.0));
p3d.push_back(Point3d(L, -L, 0.0));
p3d.push_back(Point3d(L, L, 0.0));
p3d.push_back(Point3d(-L, L, L/2));
p3d.push_back(Point3d(0, 0, -L/2));
Mat rvec_ground_truth = (Mat_<double>(3,1) << 0.3, -0.2, 0.75);
Mat tvec_ground_truth = (Mat_<double>(3,1) << 0.15, -0.2, 1.5);
vector<Point2d> p2d;
projectPoints(p3d, rvec_ground_truth, tvec_ground_truth, intrinsics, noArray(), p2d);
//add small Gaussian noise
RNG& rng = theRNG();
for (size_t i = 0; i < p2d.size(); i++)
{
p2d[i].x += rng.gaussian(5e-2);
p2d[i].y += rng.gaussian(5e-2);
}
Mat rvec_est, tvec_est;
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est, tvec_est, false, SOLVEPNP_EPNP);
{
Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone();
solvePnP(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine, true, SOLVEPNP_ITERATIVE);
cout << "\nmethod: Levenberg-Marquardt (C API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est (EPnP): " << rvec_est.t() << std::endl;
cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est (EPnP): " << tvec_est.t() << std::endl;
cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3);
EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF));
EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF));
}
{
Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone();
solvePnPRefineLM(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine);
cout << "\nmethod: Levenberg-Marquardt (C++ API)" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3);
EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF));
EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF));
}
{
Mat rvec_est_refine = rvec_est.clone(), tvec_est_refine = tvec_est.clone();
solvePnPRefineVVS(p3d, p2d, intrinsics, noArray(), rvec_est_refine, tvec_est_refine);
cout << "\nmethod: Virtual Visual Servoing" << endl;
cout << "rvec_ground_truth: " << rvec_ground_truth.t() << std::endl;
cout << "rvec_est: " << rvec_est.t() << std::endl;
cout << "rvec_est_refine: " << rvec_est_refine.t() << std::endl;
cout << "tvec_ground_truth: " << tvec_ground_truth.t() << std::endl;
cout << "tvec_est: " << tvec_est.t() << std::endl;
cout << "tvec_est_refine: " << tvec_est_refine.t() << std::endl;
EXPECT_LE(cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF), 1e-2);
EXPECT_LE(cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF), 1e-3);
EXPECT_LT(cvtest::norm(rvec_ground_truth, rvec_est_refine, NORM_INF), cvtest::norm(rvec_ground_truth, rvec_est, NORM_INF));
EXPECT_LT(cvtest::norm(tvec_ground_truth, tvec_est_refine, NORM_INF), cvtest::norm(tvec_ground_truth, tvec_est, NORM_INF));
}
}
}
TEST(Calib3d_SolvePnPRansac, minPoints)
{
//https://github.com/opencv/opencv/issues/14423
Mat matK = Mat::eye(3,3,CV_64FC1);
Mat distCoeff = Mat::zeros(1,5,CV_64FC1);
Matx31d true_rvec(0.9072420896651262, 0.09226497171882152, 0.8880772883671504);
Matx31d true_tvec(7.376333362427632, 8.434449036856979, 13.79801619778456);
{
//nb points = 5 --> ransac_kernel_method = SOLVEPNP_EPNP
Mat keypoints13D = (Mat_<float>(5, 3) << 12.00604, -2.8654366, 18.472504,
7.6863389, 4.9355154, 11.146358,
14.260933, 2.8320458, 12.582781,
3.4562225, 8.2668982, 11.300434,
15.316854, 3.7486348, 12.491116);
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, 2, CV_32FC1);
vector<Point3f> objectPoints;
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<float>(i,0) = imagesPoints[i].x;
keypoints22D.at<float>(i,1) = imagesPoints[i].y;
objectPoints.push_back(Point3f(keypoints13D.at<float>(i,0), keypoints13D.at<float>(i,1), keypoints13D.at<float>(i,2)));
}
Mat rvec = Mat::zeros(1,3,CV_64FC1);
Mat Tvec = Mat::zeros(1,3,CV_64FC1);
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
Mat rvec2, Tvec2;
solvePnP(objectPoints, imagesPoints, matK, distCoeff, rvec2, Tvec2, false, SOLVEPNP_EPNP);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-4);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-4);
EXPECT_LE(cvtest::norm(rvec, rvec2, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(Tvec, Tvec2, NORM_INF), 1e-6);
}
{
//nb points = 4 --> ransac_kernel_method = SOLVEPNP_P3P
Mat keypoints13D = (Mat_<float>(4, 3) << 12.00604, -2.8654366, 18.472504,
7.6863389, 4.9355154, 11.146358,
14.260933, 2.8320458, 12.582781,
3.4562225, 8.2668982, 11.300434);
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, 2, CV_32FC1);
vector<Point3f> objectPoints;
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<float>(i,0) = imagesPoints[i].x;
keypoints22D.at<float>(i,1) = imagesPoints[i].y;
objectPoints.push_back(Point3f(keypoints13D.at<float>(i,0), keypoints13D.at<float>(i,1), keypoints13D.at<float>(i,2)));
}
Mat rvec = Mat::zeros(1,3,CV_64FC1);
Mat Tvec = Mat::zeros(1,3,CV_64FC1);
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
Mat rvec2, Tvec2;
solvePnP(objectPoints, imagesPoints, matK, distCoeff, rvec2, Tvec2, false, SOLVEPNP_P3P);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-4);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-4);
EXPECT_LE(cvtest::norm(rvec, rvec2, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(Tvec, Tvec2, NORM_INF), 1e-6);
}
}
TEST(Calib3d_SolvePnPRansac, inputShape)
{
//https://github.com/opencv/opencv/issues/14423
Mat matK = Mat::eye(3,3,CV_64FC1);
Mat distCoeff = Mat::zeros(1,5,CV_64FC1);
Matx31d true_rvec(0.9072420896651262, 0.09226497171882152, 0.8880772883671504);
Matx31d true_tvec(7.376333362427632, 8.434449036856979, 13.79801619778456);
{
//Nx3 1-channel
Mat keypoints13D = (Mat_<float>(6, 3) << 12.00604, -2.8654366, 18.472504,
7.6863389, 4.9355154, 11.146358,
14.260933, 2.8320458, 12.582781,
3.4562225, 8.2668982, 11.300434,
10.00604, 2.8654366, 15.472504,
-4.6863389, 5.9355154, 13.146358);
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, 2, CV_32FC1);
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<float>(i,0) = imagesPoints[i].x;
keypoints22D.at<float>(i,1) = imagesPoints[i].y;
}
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
}
{
//1xN 3-channel
Mat keypoints13D(1, 6, CV_32FC3);
keypoints13D.at<Vec3f>(0,0) = Vec3f(12.00604f, -2.8654366f, 18.472504f);
keypoints13D.at<Vec3f>(0,1) = Vec3f(7.6863389f, 4.9355154f, 11.146358f);
keypoints13D.at<Vec3f>(0,2) = Vec3f(14.260933f, 2.8320458f, 12.582781f);
keypoints13D.at<Vec3f>(0,3) = Vec3f(3.4562225f, 8.2668982f, 11.300434f);
keypoints13D.at<Vec3f>(0,4) = Vec3f(10.00604f, 2.8654366f, 15.472504f);
keypoints13D.at<Vec3f>(0,5) = Vec3f(-4.6863389f, 5.9355154f, 13.146358f);
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, keypoints13D.cols, CV_32FC2);
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<Vec2f>(0,i) = Vec2f(imagesPoints[i].x, imagesPoints[i].y);
}
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
}
{
//Nx1 3-channel
Mat keypoints13D(6, 1, CV_32FC3);
keypoints13D.at<Vec3f>(0,0) = Vec3f(12.00604f, -2.8654366f, 18.472504f);
keypoints13D.at<Vec3f>(1,0) = Vec3f(7.6863389f, 4.9355154f, 11.146358f);
keypoints13D.at<Vec3f>(2,0) = Vec3f(14.260933f, 2.8320458f, 12.582781f);
keypoints13D.at<Vec3f>(3,0) = Vec3f(3.4562225f, 8.2668982f, 11.300434f);
keypoints13D.at<Vec3f>(4,0) = Vec3f(10.00604f, 2.8654366f, 15.472504f);
keypoints13D.at<Vec3f>(5,0) = Vec3f(-4.6863389f, 5.9355154f, 13.146358f);
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, keypoints13D.cols, CV_32FC2);
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<Vec2f>(i,0) = Vec2f(imagesPoints[i].x, imagesPoints[i].y);
}
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
}
{
//vector<Point3f>
vector<Point3f> keypoints13D;
keypoints13D.push_back(Point3f(12.00604f, -2.8654366f, 18.472504f));
keypoints13D.push_back(Point3f(7.6863389f, 4.9355154f, 11.146358f));
keypoints13D.push_back(Point3f(14.260933f, 2.8320458f, 12.582781f));
keypoints13D.push_back(Point3f(3.4562225f, 8.2668982f, 11.300434f));
keypoints13D.push_back(Point3f(10.00604f, 2.8654366f, 15.472504f));
keypoints13D.push_back(Point3f(-4.6863389f, 5.9355154f, 13.146358f));
vector<Point2f> keypoints22D;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, keypoints22D);
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
}
{
//vector<Point3d>
vector<Point3d> keypoints13D;
keypoints13D.push_back(Point3d(12.00604f, -2.8654366f, 18.472504f));
keypoints13D.push_back(Point3d(7.6863389f, 4.9355154f, 11.146358f));
keypoints13D.push_back(Point3d(14.260933f, 2.8320458f, 12.582781f));
keypoints13D.push_back(Point3d(3.4562225f, 8.2668982f, 11.300434f));
keypoints13D.push_back(Point3d(10.00604f, 2.8654366f, 15.472504f));
keypoints13D.push_back(Point3d(-4.6863389f, 5.9355154f, 13.146358f));
vector<Point2d> keypoints22D;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, keypoints22D);
Mat rvec, Tvec;
solvePnPRansac(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-6);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-6);
}
}
TEST(Calib3d_SolvePnP, inputShape)
{
//https://github.com/opencv/opencv/issues/14423
Mat matK = Mat::eye(3,3,CV_64FC1);
Mat distCoeff = Mat::zeros(1,5,CV_64FC1);
Matx31d true_rvec(0.407, 0.092, 0.88);
Matx31d true_tvec(0.576, -0.43, 1.3798);
vector<Point3d> objectPoints;
const double L = 0.5;
objectPoints.push_back(Point3d(-L, -L, L));
objectPoints.push_back(Point3d( L, -L, L));
objectPoints.push_back(Point3d( L, L, L));
objectPoints.push_back(Point3d(-L, L, L));
objectPoints.push_back(Point3d(-L, -L, -L));
objectPoints.push_back(Point3d( L, -L, -L));
const int methodsCount = 6;
int methods[] = {SOLVEPNP_ITERATIVE, SOLVEPNP_EPNP, SOLVEPNP_P3P, SOLVEPNP_AP3P, SOLVEPNP_IPPE, SOLVEPNP_IPPE_SQUARE};
for (int method = 0; method < methodsCount; method++)
{
if (methods[method] == SOLVEPNP_IPPE_SQUARE)
{
objectPoints[0] = Point3d(-L, L, 0);
objectPoints[1] = Point3d( L, L, 0);
objectPoints[2] = Point3d( L, -L, 0);
objectPoints[3] = Point3d(-L, -L, 0);
}
{
//Nx3 1-channel
Mat keypoints13D;
if (methods[method] == SOLVEPNP_P3P || methods[method] == SOLVEPNP_AP3P ||
methods[method] == SOLVEPNP_IPPE || methods[method] == SOLVEPNP_IPPE_SQUARE)
{
keypoints13D = Mat(4, 3, CV_32FC1);
}
else
{
keypoints13D = Mat(6, 3, CV_32FC1);
}
for (int i = 0; i < keypoints13D.rows; i++)
{
keypoints13D.at<float>(i,0) = static_cast<float>(objectPoints[i].x);
keypoints13D.at<float>(i,1) = static_cast<float>(objectPoints[i].y);
keypoints13D.at<float>(i,2) = static_cast<float>(objectPoints[i].z);
}
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, 2, CV_32FC1);
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<float>(i,0) = imagesPoints[i].x;
keypoints22D.at<float>(i,1) = imagesPoints[i].y;
}
Mat rvec, Tvec;
solvePnP(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec, false, methods[method]);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-3);
}
{
//1xN 3-channel
Mat keypoints13D;
if (methods[method] == SOLVEPNP_P3P || methods[method] == SOLVEPNP_AP3P ||
methods[method] == SOLVEPNP_IPPE || methods[method] == SOLVEPNP_IPPE_SQUARE)
{
keypoints13D = Mat(1, 4, CV_32FC3);
}
else
{
keypoints13D = Mat(1, 6, CV_32FC3);
}
for (int i = 0; i < keypoints13D.cols; i++)
{
keypoints13D.at<Vec3f>(0,i) = Vec3f(static_cast<float>(objectPoints[i].x),
static_cast<float>(objectPoints[i].y),
static_cast<float>(objectPoints[i].z));
}
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, keypoints13D.cols, CV_32FC2);
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<Vec2f>(0,i) = Vec2f(imagesPoints[i].x, imagesPoints[i].y);
}
Mat rvec, Tvec;
solvePnP(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec, false, methods[method]);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-3);
}
{
//Nx1 3-channel
Mat keypoints13D;
if (methods[method] == SOLVEPNP_P3P || methods[method] == SOLVEPNP_AP3P ||
methods[method] == SOLVEPNP_IPPE || methods[method] == SOLVEPNP_IPPE_SQUARE)
{
keypoints13D = Mat(4, 1, CV_32FC3);
}
else
{
keypoints13D = Mat(6, 1, CV_32FC3);
}
for (int i = 0; i < keypoints13D.rows; i++)
{
keypoints13D.at<Vec3f>(i,0) = Vec3f(static_cast<float>(objectPoints[i].x),
static_cast<float>(objectPoints[i].y),
static_cast<float>(objectPoints[i].z));
}
vector<Point2f> imagesPoints;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, imagesPoints);
Mat keypoints22D(keypoints13D.rows, keypoints13D.cols, CV_32FC2);
for (int i = 0; i < static_cast<int>(imagesPoints.size()); i++)
{
keypoints22D.at<Vec2f>(i,0) = Vec2f(imagesPoints[i].x, imagesPoints[i].y);
}
Mat rvec, Tvec;
solvePnP(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec, false, methods[method]);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-3);
}
{
//vector<Point3f>
vector<Point3f> keypoints13D;
const int nbPts = (methods[method] == SOLVEPNP_P3P || methods[method] == SOLVEPNP_AP3P ||
methods[method] == SOLVEPNP_IPPE || methods[method] == SOLVEPNP_IPPE_SQUARE) ? 4 : 6;
for (int i = 0; i < nbPts; i++)
{
keypoints13D.push_back(Point3f(static_cast<float>(objectPoints[i].x),
static_cast<float>(objectPoints[i].y),
static_cast<float>(objectPoints[i].z)));
}
vector<Point2f> keypoints22D;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, keypoints22D);
Mat rvec, Tvec;
solvePnP(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec, false, methods[method]);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-3);
}
{
//vector<Point3d>
vector<Point3d> keypoints13D;
const int nbPts = (methods[method] == SOLVEPNP_P3P || methods[method] == SOLVEPNP_AP3P ||
methods[method] == SOLVEPNP_IPPE || methods[method] == SOLVEPNP_IPPE_SQUARE) ? 4 : 6;
for (int i = 0; i < nbPts; i++)
{
keypoints13D.push_back(objectPoints[i]);
}
vector<Point2d> keypoints22D;
projectPoints(keypoints13D, true_rvec, true_tvec, matK, distCoeff, keypoints22D);
Mat rvec, Tvec;
solvePnP(keypoints13D, keypoints22D, matK, distCoeff, rvec, Tvec, false, methods[method]);
EXPECT_LE(cvtest::norm(true_rvec, rvec, NORM_INF), 1e-3);
EXPECT_LE(cvtest::norm(true_tvec, Tvec, NORM_INF), 1e-3);
}
}
}
}} // namespace
Reference in a new issue Copy permalink