mirror of https://github.com/opencv/opencv.git
Open Source Computer Vision Library
https://opencv.org/
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2323 lines
91 KiB
2323 lines
91 KiB
/*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" |
|
#include "opencv2/core/utils/logger.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 = vector<Point3f>(p3f_.begin(), p3f_.end()); |
|
p2f = vector<Point2f>(p2f_.begin(), p2f_.end()); |
|
} |
|
|
|
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); |
|
} |
|
} |
|
} |
|
|
|
bool hasNan(const cv::Mat& mat) |
|
{ |
|
bool has = false; |
|
if (mat.type() == CV_32F) |
|
{ |
|
for(int i = 0; i < static_cast<int>(mat.total()); i++) |
|
has |= cvIsNaN(mat.at<float>(i)) != 0; |
|
} |
|
else if (mat.type() == CV_64F) |
|
{ |
|
for(int i = 0; i < static_cast<int>(mat.total()); i++) |
|
has |= cvIsNaN(mat.at<double>(i)) != 0; |
|
} |
|
else |
|
{ |
|
has = true; |
|
CV_LOG_ERROR(NULL, "check hasNan called with unsupported type!"); |
|
} |
|
|
|
return has; |
|
} |
|
|
|
TEST(AP3P, ctheta1p_nan_23607) |
|
{ |
|
// the task is not well defined and may not converge (empty R, t) or should |
|
// converge to some non-NaN solution |
|
const std::array<cv::Point2d, 3> cameraPts = { |
|
cv::Point2d{0.042784865945577621, 0.59844839572906494}, |
|
cv::Point2d{-0.028428621590137482, 0.60354739427566528}, |
|
cv::Point2d{0.0046037044376134872, 0.70674681663513184} |
|
}; |
|
const std::array<cv::Point3d, 3> modelPts = { |
|
cv::Point3d{-0.043258000165224075, 0.020459245890378952, -0.0069921980611979961}, |
|
cv::Point3d{-0.045648999512195587, 0.0029820732306689024, 0.0079000638797879219}, |
|
cv::Point3d{-0.043276999145746231, -0.013622495345771313, 0.0080113131552934647} |
|
}; |
|
|
|
std::vector<Mat> R, t; |
|
solveP3P(modelPts, cameraPts, Mat::eye(3, 3, CV_64F), Mat(), R, t, SOLVEPNP_AP3P); |
|
|
|
EXPECT_EQ(R.size(), 2ul); |
|
EXPECT_EQ(t.size(), 2ul); |
|
|
|
// Try apply rvec and tvec to get model points from camera points. |
|
Mat pts = Mat(modelPts).reshape(1, 3); |
|
Mat expected = Mat(cameraPts).reshape(1, 3); |
|
for (size_t i = 0; i < R.size(); ++i) { |
|
EXPECT_TRUE(!hasNan(R[i])); |
|
EXPECT_TRUE(!hasNan(t[i])); |
|
|
|
Mat transform; |
|
cv::Rodrigues(R[i], transform); |
|
Mat res = pts * transform.t(); |
|
for (int j = 0; j < 3; ++j) { |
|
res.row(j) += t[i].reshape(1, 1); |
|
res.row(j) /= res.row(j).at<double>(2); |
|
} |
|
EXPECT_LE(cvtest::norm(res.colRange(0, 2), expected, NORM_INF), 3e-16); |
|
} |
|
} |
|
|
|
}} // namespace
|
|
|