generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Solvepnpransac() returns boolean

Browse Source
This commit is contained in:
edgarriba
2014-07-09 10:37:37 +02:00
parent ce07024a44
commit ba5a8ee299
2 changed files with 29 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -154,7 +154,7 @@ CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints,
bool useExtrinsicGuess = false, int flags = ITERATIVE ); bool useExtrinsicGuess = false, int flags = ITERATIVE );
//! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible. //! computes the camera pose from a few 3D points and the corresponding projections. The outliers are possible.
CV_EXPORTS_W void solvePnPRansac( InputArray objectPoints, InputArray imagePoints, CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
InputArray cameraMatrix, InputArray distCoeffs, InputArray cameraMatrix, InputArray distCoeffs,
OutputArray rvec, OutputArray tvec, OutputArray rvec, OutputArray tvec,
bool useExtrinsicGuess = false, int iterationsCount = 100, bool useExtrinsicGuess = false, int iterationsCount = 100,

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

@@ -98,7 +98,7 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
return false; return false;
} }
namespace cv /*namespace cv
{ {
namespace pnpransac namespace pnpransac
{ {
@@ -271,7 +271,7 @@ namespace cv
RNG PnPSolver::generator; RNG PnPSolver::generator;
} }
} }*/
class PnPRansacCallback : public PointSetRegistrator::Callback class PnPRansacCallback : public PointSetRegistrator::Callback
{ {
@@ -283,7 +283,7 @@ public:
rvec(_rvec), tvec(_tvec) {} rvec(_rvec), tvec(_tvec) {}
/* Pre: True */ /* Pre: True */
/* Post: compute _model with given points an eturn number of found models */ /* Post: compute _model with given points an return number of found models */
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
{ {
Mat opoints = _m1.getMat(), ipoints = _m2.getMat(); Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
@@ -291,15 +291,9 @@ public:
bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs, bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
rvec, tvec, useExtrinsicGuess, flags ); rvec, tvec, useExtrinsicGuess, flags );
Mat _local_model;
if(correspondence) cv::hconcat(rvec, tvec, _local_model);
{ _local_model.copyTo(_model);
Mat _local_model;
_local_model.push_back(rvec);
_local_model.push_back(tvec);
_local_model.copyTo(_model);
}
return correspondence; return correspondence;
} }
@@ -312,8 +306,8 @@ public:
Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat(); Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat();
int i, count = opoints.cols; int i, count = opoints.cols;
Mat _rvec = model.rowRange(0,3); Mat _rvec = model.col(0);
Mat _tvec = model.rowRange(3, 6); Mat _tvec = model.col(1);
Mat projpoints(count, 2, CV_32FC1); Mat projpoints(count, 2, CV_32FC1);
cv::projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints); cv::projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
@@ -338,7 +332,7 @@ public:
Mat tvec; Mat tvec;
}; };
void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, bool cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, float confidence, int iterationsCount, float reprojectionError, float confidence,
@@ -355,34 +349,16 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F); CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2); CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
_rvec.create(3, 1, CV_64FC1); _rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1); _tvec.create(3, 1, CV_64FC1);
Mat rvec, tvec; Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
if (useExtrinsicGuess) // use given rvec & tvec Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
{ cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, useExtrinsicGuess, rvec, tvec);
rvec = _rvec.getMat();
tvec = _tvec.getMat();
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags, int model_points = 6; // minimum of number of model points
useExtrinsicGuess, rvec, tvec);
}
else
{
rvec = Mat(3, 1, CV_64FC1);
tvec = Mat(3, 1, CV_64FC1);
cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, flags,
useExtrinsicGuess, rvec, tvec);
}
int model_points = 3; // minimum of number of model points
double param1 = reprojectionError ; // reprojection error double param1 = reprojectionError ; // reprojection error
double param2 = confidence; // confidence double param2 = confidence; // confidence
int param3 = iterationsCount; // number maximum iterations int param3 = iterationsCount; // number maximum iterations
@@ -391,11 +367,20 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1); cv::Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
// call Ransac // call Ransac
int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3) int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(_opoints, _ipoints, _local_model, _mask_local_inliers);
->run(_opoints, _ipoints, _local_model, _mask_local_inliers);
_rvec.assign(_local_model.rowRange(0,3)); // output rotation vector if( result <= 0 || _local_model.rows <= 0)
_tvec.assign(_local_model.rowRange(3,6)); // output translation vector {
_rvec.assign(rvec); // output rotation vector
_tvec.assign(tvec); // output translation vector
return false;
}
else
{
_rvec.assign(_local_model.col(0)); // output rotation vector
_tvec.assign(_local_model.col(1)); // output translation vector
}
Mat _local_inliers; Mat _local_inliers;
int count = 0; int count = 0;
@@ -452,5 +437,5 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
if( _inliers.needed() ) if( _inliers.needed() )
_inliers.release(); _inliers.release();
}*/ }*/
return; return true;
} }