mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-08-05 22:19:14 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #9086 from catree:improve_solvePnPRansac

Browse Source
This commit is contained in:
Alexander Alekhin 2017-07-14 17:28:47 +00:00
commit 42c0914382
2 changed files with 62 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
modules/calib3d/include/opencv2/calib3d.hpp
View File

@ -627,6 +627,13 @@ makes the function resistant to outliers.
@note
- An example of how to use solvePNPRansac for object detection can be found at
opencv_source_code/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/
- The default method used to estimate the camera pose for the Minimal Sample Sets step
is #SOLVEPNP_EPNP. Exceptions are:
- if you choose #SOLVEPNP_P3P or #SOLVEPNP_AP3P, these methods will be used.
- if the number of input points is equal to 4, #SOLVEPNP_P3P is used.
- The method used to estimate the camera pose using all the inliers is defined by the
flags parameters unless it is equal to #SOLVEPNP_P3P or #SOLVEPNP_AP3P. In this case,
the method #SOLVEPNP_EPNP will be used instead.
*/
CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
InputArray cameraMatrix, InputArray distCoeffs,

77
modules/calib3d/src/solvepnp.cpp
View File

@ -249,7 +249,7 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
ipoints = ipoints0;
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)) );
CV_Assert( npoints >= 4 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
@ -279,6 +279,31 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
ransac_kernel_method = SOLVEPNP_P3P;
}
if( model_points == npoints )
{
bool result = solvePnP(opoints, ipoints, cameraMatrix, distCoeffs, _rvec, _tvec, useExtrinsicGuess, ransac_kernel_method);
if(!result)
{
if( _inliers.needed() )
_inliers.release();
return false;
}
if(_inliers.needed())
{
_inliers.create(npoints, 1, CV_32S);
Mat _local_inliers = _inliers.getMat();
for(int i = 0; i < npoints; i++)
{
_local_inliers.at<int>(i) = i;
}
}
return true;
}
Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec);
@ -293,26 +318,6 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
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 = opoints.reshape(3);
ipoints = ipoints.reshape(2);
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 || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
}
if( result <= 0 || _local_model.rows <= 0)
{
_rvec.assign(rvec); // output rotation vector
@ -323,10 +328,38 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
return false;
}
else
vector<Point3d> opoints_inliers;
vector<Point2d> ipoints_inliers;
opoints = opoints.reshape(3);
ipoints = ipoints.reshape(2);
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, useExtrinsicGuess,
(flags == SOLVEPNP_P3P || flags == SOLVEPNP_AP3P) ? SOLVEPNP_EPNP : flags) ? 1 : -1;
if( result <= 0 )
{
_rvec.assign(_local_model.col(0)); // output rotation vector
_tvec.assign(_local_model.col(1)); // output translation vector
if( _inliers.needed() )
_inliers.release();
return false;
}
else
{
_rvec.assign(rvec); // output rotation vector
_tvec.assign(tvec); // output translation vector
}
if(_inliers.needed())