mirror of
https://github.com/opencv/opencv.git
synced 2024-12-15 09:49:13 +08:00
114c23e411
Fix crash in ap3p #23607 ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [ ] I agree to contribute to the project under Apache 2 License. - [ ] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake
2324 lines
91 KiB
C++
2324 lines
91 KiB
C++
/*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
|