Skip to content

O
opencv
Commit 8cf45ce0 authored by Vadim Pisarevsky's avatar Vadim Pisarevsky
Browse files
Options

Merge pull request #3828 from vpisarev:fix_win32_perf_calib3d_solvepnp_failure

parents 3c54c50c ca19ae8b
Hide whitespace changes
Inline Side-by-side
Showing with 104 additions and 68 deletions
modules/calib3d/perf/perf_pnp.cpp
View file @ 8cf45ce0
......@@ -19,8 +19,8 @@ typedef perf::TestBaseWithParam<int> PointsNum;
PERF_TEST_P(PointsNum_Algo, solvePnP,
testing::Combine(
testing::Values(4, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP)
testing::Values(5, 3*9, 7*13), //TODO: find why results on 4 points are too unstable
testing::Values((int)SOLVEPNP_ITERATIVE, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_UPNP, (int)SOLVEPNP_DLS)
)
)
{
......@@ -64,13 +64,15 @@ PERF_TEST_P(PointsNum_Algo, solvePnP,
PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
testing::Combine(
testing::Values(4), //TODO: find why results on 4 points are too unstable
testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
testing::Values(5),
testing::Values((int)SOLVEPNP_P3P, (int)SOLVEPNP_EPNP, (int)SOLVEPNP_DLS, (int)SOLVEPNP_UPNP)
)
)
{
int pointsNum = get<0>(GetParam());
pnpAlgo algo = get<1>(GetParam());
if( algo == SOLVEPNP_P3P )
pointsNum = 4;
vector<Point2f> points2d(pointsNum);
vector<Point3f> points3d(pointsNum);
......@@ -92,7 +94,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
//add noise
Mat noise(1, (int)points2d.size(), CV_32FC2);
randu(noise, 0, 0.01);
randu(noise, -0.001, 0.001);
add(points2d, noise, points2d);
declare.in(points3d, points2d);
......@@ -107,7 +109,7 @@ PERF_TEST_P(PointsNum_Algo, solvePnPSmallPoints,
SANITY_CHECK(tvec, 1e-2);
}
PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(4, 3*9, 7*13))
PERF_TEST_P(PointsNum, DISABLED_SolvePnPRansac, testing::Values(5, 3*9, 7*13))
{
int count = GetParam();
......
modules/calib3d/src/epnp.cpp
View file @ 8cf45ce0
......@@ -2,7 +2,10 @@
#include "precomp.hpp"
#include "epnp.h"
epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints)
namespace cv
{
epnp::epnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints)
{
if (cameraMatrix.depth() == CV_32F)
init_camera_parameters<float>(cameraMatrix);
......@@ -17,14 +20,14 @@ epnp::epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& i
if (opoints.depth() == ipoints.depth())
{
if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2f>(opoints, ipoints);
init_points<Point3f,Point2f>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2d>(opoints, ipoints);
init_points<Point3d,Point2d>(opoints, ipoints);
}
else if (opoints.depth() == CV_32F)
init_points<cv::Point3f,cv::Point2d>(opoints, ipoints);
init_points<Point3f,Point2d>(opoints, ipoints);
else
init_points<cv::Point3d,cv::Point2f>(opoints, ipoints);
init_points<Point3d,Point2f>(opoints, ipoints);
alphas.resize(4 * number_of_correspondences);
pcs.resize(3 * number_of_correspondences);
......@@ -144,7 +147,7 @@ void epnp::compute_pcs(void)
}
}
void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
void epnp::compute_pose(Mat& R, Mat& t)
{
choose_control_points();
compute_barycentric_coordinates();
......@@ -189,8 +192,8 @@ void epnp::compute_pose(cv::Mat& R, cv::Mat& t)
if (rep_errors[2] < rep_errors[1]) N = 2;
if (rep_errors[3] < rep_errors[N]) N = 3;
cv::Mat(3, 1, CV_64F, ts[N]).copyTo(t);
cv::Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
Mat(3, 1, CV_64F, ts[N]).copyTo(t);
Mat(3, 3, CV_64F, Rs[N]).copyTo(R);
}
void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
......@@ -621,3 +624,5 @@ void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
pX[i] = (pb[i] - sum) / A2[i];
}
}
}
modules/calib3d/src/epnp.h
View file @ 8cf45ce0
......@@ -4,6 +4,9 @@
#include "precomp.hpp"
#include "opencv2/core/core_c.h"
namespace cv
{
class epnp {
public:
epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
......@@ -78,4 +81,6 @@ class epnp {
double * A1, * A2;
};
}
#endif
modules/calib3d/src/fundam.cpp
View file @ 8cf45ce0
......@@ -69,20 +69,6 @@ static bool haveCollinearPoints( const Mat& m, int count )
}
template<typename T> int compressPoints( T* ptr, const uchar* mask, int mstep, int count )
{
int i, j;
for( i = j = 0; i < count; i++ )
if( mask[i*mstep] )
{
if( i > j )
ptr[j] = ptr[i];
j++;
}
return j;
}
class HomographyEstimatorCallback : public PointSetRegistrator::Callback
{
public:
......@@ -322,8 +308,8 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
if( result && npoints > 4 )
{
compressPoints( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
npoints = compressPoints( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
compressElems( src.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
npoints = compressElems( dst.ptr<Point2f>(), tempMask.ptr<uchar>(), 1, npoints );
if( npoints > 0 )
{
Mat src1 = src.rowRange(0, npoints);
......
modules/calib3d/src/precomp.hpp
View file @ 8cf45ce0
......@@ -102,6 +102,19 @@ CV_EXPORTS Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<Po
CV_EXPORTS Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& cb,
int modelPoints, double confidence=0.99, int maxIters=1000 );
template<typename T> inline int compressElems( T* ptr, const uchar* mask, int mstep, int count )
{
int i, j;
for( i = j = 0; i < count; i++ )
if( mask[i*mstep] )
{
if( i > j )
ptr[j] = ptr[i];
j++;
}
return j;
}
}
#endif
modules/calib3d/src/solvepnp.cpp
View file @ 8cf45ce0
......@@ -48,41 +48,43 @@
#include "opencv2/calib3d/calib3d_c.h"
#include <iostream>
using namespace cv;
bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
namespace cv
{
bool solvePnP( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
{
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
_rvec.create(3, 1, CV_64F);
_tvec.create(3, 1, CV_64F);
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
Mat cameraMatrix = Mat_<double>(_cameraMatrix.getMat()), distCoeffs = Mat_<double>(_distCoeffs.getMat());
if (flags == SOLVEPNP_EPNP)
if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
{
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
epnp PnP(cameraMatrix, opoints, undistortedPoints);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
PnP.compute_pose(R, tvec);
cv::Rodrigues(R, rvec);
Rodrigues(R, rvec);
return true;
}
else if (flags == SOLVEPNP_P3P)
{
CV_Assert( npoints == 4);
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
p3p P3Psolver(cameraMatrix);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result)
cv::Rodrigues(R, rvec);
Rodrigues(R, rvec);
return result;
}
else if (flags == SOLVEPNP_ITERATIVE)
......@@ -95,32 +97,32 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
&c_rvec, &c_tvec, useExtrinsicGuess );
return true;
}
else if (flags == SOLVEPNP_DLS)
/*else if (flags == SOLVEPNP_DLS)
{
cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
Mat undistortedPoints;
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
dls PnP(opoints, undistortedPoints);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = PnP.compute_pose(R, tvec);
if (result)
cv::Rodrigues(R, rvec);
Rodrigues(R, rvec);
return result;
}
else if (flags == SOLVEPNP_UPNP)
{
upnp PnP(cameraMatrix, opoints, ipoints);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
double f = PnP.compute_pose(R, tvec);
cv::Rodrigues(R, rvec);
Rodrigues(R, rvec);
if(cameraMatrix.type() == CV_32F)
cameraMatrix.at<float>(0,0) = cameraMatrix.at<float>(1,1) = (float)f;
else
cameraMatrix.at<double>(0,0) = cameraMatrix.at<double>(1,1) = f;
return true;
}
}*/
else
CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
return false;
......@@ -131,7 +133,7 @@ class PnPRansacCallback : public PointSetRegistrator::Callback
public:
PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::SOLVEPNP_ITERATIVE,
PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=SOLVEPNP_ITERATIVE,
bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
: cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
rvec(_rvec), tvec(_tvec) {}
......@@ -142,12 +144,11 @@ public:
{
Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
rvec, tvec, useExtrinsicGuess, flags );
Mat _local_model;
cv::hconcat(rvec, tvec, _local_model);
hconcat(rvec, tvec, _local_model);
_local_model.copyTo(_model);
return correspondence;
......@@ -166,7 +167,7 @@ public:
Mat projpoints(count, 2, CV_32FC1);
cv::projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
......@@ -175,7 +176,7 @@ public:
float* err = _err.getMat().ptr<float>();
for ( i = 0; i < count; ++i)
err[i] = (float)cv::norm( ipoints_ptr[i] - projpoints_ptr[i] );
err[i] = (float)norm( ipoints_ptr[i] - projpoints_ptr[i] );
}
......@@ -188,7 +189,7 @@ public:
Mat tvec;
};
bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, double confidence,
......@@ -214,23 +215,45 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec);
int model_points = 5;
int ransac_kernel_method = SOLVEPNP_EPNP;
int model_points = 4; // minimum of number of model points
if( flags == cv::SOLVEPNP_ITERATIVE ) model_points = 6;
else if( flags == cv::SOLVEPNP_UPNP ) model_points = 6;
else if( flags == cv::SOLVEPNP_EPNP ) model_points = 5;
if( npoints == 4 )
{
model_points = 4;
ransac_kernel_method = SOLVEPNP_P3P;
}
Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec);
double param1 = reprojectionError; // reprojection error
double param2 = confidence; // confidence
int param3 = iterationsCount; // number maximum iterations
cv::Mat _local_model(3, 2, CV_64FC1);
cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
Mat _local_model(3, 2, CV_64FC1);
Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
// call Ransac
int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
int result = createRANSACPointSetRegistrator(cb, model_points,
param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
if( result > 0 )
{
vector<Point3d> opoints_inliers;
vector<Point2d> ipoints_inliers;
opoints.convertTo(opoints_inliers, CV_64F);
ipoints.convertTo(ipoints_inliers, CV_64F);
const uchar* mask = _mask_local_inliers.ptr<uchar>();
int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints);
compressElems(&ipoints_inliers[0], mask, 1, npoints);
opoints_inliers.resize(npoints1);
ipoints_inliers.resize(npoints1);
result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1;
}
if( result <= 0 || _local_model.rows <= 0)
{
......@@ -260,3 +283,5 @@ bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
}
return true;
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment