Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

New Ransac implementation WORKING

Browse Source
pull/3042/head
edgarriba 11 years ago
parent e0c4936c99
commit dd52d1b526
1 changed files with 63 additions and 50 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 113
      modules/calib3d/src/solvepnp.cpp

113
modules/calib3d/src/solvepnp.cpp
Unescape View File

@ -276,12 +276,11 @@ namespace cv
class PnPRansacCallback : public PointSetRegistrator::Callback
{
public:
bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
{
// which kind of checking??
return true;
}
PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=cv::ITERATIVE,
bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
: cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
rvec(_rvec), tvec(_tvec) {}
/* Pre: True */
/* Post: compute _model with given points an eturn number of found models */
@ -289,18 +288,20 @@ public:
{
Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
Mat cameraMatrix = _model.getMat(0);
Mat distCoeffs = _model.getMat(1);
Mat rvec = _model.getMat(2);
Mat tvec = _model.getMat(3);
int flags = _model.getMat(4).at<int>(0);
bool correspondence = cv::solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
rvec, tvec, useExtrinsicGuess, flags );
bool useExtrinsicGuess = true;
if(correspondence)
{
Mat _local_model;
_local_model.push_back(rvec);
_local_model.push_back(tvec);
bool correspondence = cv::solvePnP( opoints, ipoints,
cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags );
_local_model.copyTo(_model);
}
return 1;
return correspondence;
}
/* Pre: True */
@ -308,28 +309,33 @@ public:
void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
{
Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
Mat cameraMatrix = _model.getMat(0);
Mat distCoeffs = _model.getMat(1);
Mat rvec = _model.getMat(2);
Mat tvec = _model.getMat(3);
Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat();
int i, count = opoints.cols;
Mat _rvec = model.rowRange(0,3);
Mat _tvec = model.rowRange(3, 6);
Mat projpoints(count, 2, CV_64FC1);
cv::projectPoints(opoints, rvec, tvec, cameraMatrix, distCoeffs, projpoints);
Mat projpoints(count, 2, CV_32FC1);
cv::projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
_err.create(count, 1, CV_64FC1);
_err.create(count, 1, CV_32FC1);
float* err = _err.getMat().ptr<float>();
for ( i = 0; i < count; ++i)
err[i] = cv::norm( ipoints_ptr[i] - projpoints_ptr[i] );
}
Mat cameraMatrix;
Mat distCoeffs;
int flags;
bool useExtrinsicGuess;
Mat rvec;
Mat tvec;
};
void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
@ -338,7 +344,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
int iterationsCount, float reprojectionError, int minInliersCount,
OutputArray _inliers, int flags)
{
// NO CHANGES
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
@ -349,11 +355,6 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
CV_Assert(ipoints.depth() == CV_32F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
Mat rvec = _rvec.getMat();
Mat tvec = _tvec.getMat();
Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
if (minInliersCount <= 0)
@ -365,47 +366,59 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
params.useExtrinsicGuess = useExtrinsicGuess;
params.camera.init(cameraMatrix, distCoeffs);
params.flags = flags;
// END NO CHANGES
cv::Mat flag(1, 1, CV_8UC1);
flag.at<int>(0) = params.flags;
Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
// Embed input model to a Mat
std::vector<cv::Mat> _model;
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
_model.push_back(_cameraMatrix.getMat()); // 3x3
_model.push_back(_distCoeffs.getMat()); // 4x1
_model.push_back(_rvec.getMat()); // 3x1
_model.push_back(_tvec.getMat()); // 3x1
_model.push_back(flag); // 1x1
Mat rvec, tvec;
cv::Mat local_inliers;
if (useExtrinsicGuess) // use given rvec & tvec
{
rvec = _rvec.getMat();
tvec = _tvec.getMat();
cb = makePtr<PnPRansacCallback>( params.camera.intrinsics, params.camera.distortion,
params.flags, params.useExtrinsicGuess, rvec, tvec);
}
else
{
rvec = Mat(3, 1, CV_64FC1);
tvec = Mat(3, 1, CV_64FC1);
Ptr<PointSetRegistrator::Callback> cb = makePtr<PnPRansacCallback>(); // pointer to callback
cb = makePtr<PnPRansacCallback>( params.camera.intrinsics, params.camera.distortion,
params.flags, params.useExtrinsicGuess, rvec, tvec);
}
int model_points = 7; // number of model points. From fundamentalMatrix, must change
int model_points = 7; // number of model points
double param1 = params.reprojectionError ; // reprojection error
double param2 = 0.99; // confidence
int param3 = params.iterationsCount; // number maximum iterations
cv::Mat _local_model(3, 2, CV_64FC1);
cv::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);
// NO COMPILE, IT DOESN'T LIKE vector<Mat> in run
int result = createRANSACPointSetRegistrator(cb, model_points, param1, param2, param3)->run(objectPoints, imagePoints, _model, local_inliers);
_rvec.assign(_local_model.rowRange(0,3)); // output rotation vector
_tvec.assign(_local_model.rowRange(3,6)); // output translation vector
_rvec.assign(_model.at<cv::Mat>(2)); // output rotation vector
_tvec.assign(_model.at<cv::Mat>(3)); // output translation vector
//std::cout << _mask_local_inliers.type() << std::endl;
// output inliers vector
Mat _local_inliers;
int count = 0;
for (int i = 0; i < local_inliers.rows; ++i)
for (int i = 0; i < _mask_local_inliers.rows; ++i)
{
if(local_inliers.at<int>(i) == 1)
if((int)_mask_local_inliers.at<uchar>(i) == 1) // inliers mask
{
cv::Mat & inliers = _inliers.getMat().at<int>(count) = i;
_local_inliers.push_back(count); // output inliers vector
count++;
}
}
_local_inliers.copyTo(_inliers);
// OLD IMPLEMENTATION

Write Preview
Loading…
Cancel
Save