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

Merge pull request #4053 from vpisarev:calib3d_fixes

Browse Source
This commit is contained in:
Vadim Pisarevsky
2015-05-26 11:23:50 +00:00
parent b1a8e4f760 bb2e40b999
commit dc587992a0
8 changed files with 338 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
modules/calib3d/src/calibration.cpp
View File

@@ -820,9 +820,9 @@ CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
{
dpdk_p[5] = fx*x*cdist*(-icdist2)*icdist2*r2;
dpdk_p[dpdk_step+5] = fy*y*cdist*(-icdist2)*icdist2*r2;
dpdk_p[6] = fx*x*icdist2*cdist*(-icdist2)*icdist2*r4;
dpdk_p[6] = fx*x*cdist*(-icdist2)*icdist2*r4;
dpdk_p[dpdk_step+6] = fy*y*cdist*(-icdist2)*icdist2*r4;
dpdk_p[7] = fx*x*icdist2*cdist*(-icdist2)*icdist2*r6;
dpdk_p[7] = fx*x*cdist*(-icdist2)*icdist2*r6;
dpdk_p[dpdk_step+7] = fy*y*cdist*(-icdist2)*icdist2*r6;
if( _dpdk->cols > 8 )
{

7
modules/calib3d/src/ptsetreg.cpp
View File

@@ -80,7 +80,7 @@ public:
int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
: cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters)
{
checkPartialSubsets = true;
checkPartialSubsets = false;
}
int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
@@ -145,6 +145,9 @@ public:
ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 ))
{
// we may have selected some bad points;
// so, let's remove some of them randomly
i = rng.uniform(0, i+1);
iters++;
continue;
}
@@ -206,7 +209,7 @@ public:
int i, goodCount, nmodels;
if( count > modelPoints )
{
bool found = getSubset( m1, m2, ms1, ms2, rng );
bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
if( !found )
{
if( iter == 0 )

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

@@ -59,9 +59,33 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
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 = Mat_<double>(_cameraMatrix.getMat()), distCoeffs = Mat_<double>(_distCoeffs.getMat());
Mat rvec, tvec;
if( flags != SOLVEPNP_ITERATIVE )
useExtrinsicGuess = false;
if( useExtrinsicGuess )
{
int rtype = _rvec.type(), ttype = _tvec.type();
Size rsize = _rvec.size(), tsize = _tvec.size();
CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
(ttype == CV_32F || ttype == CV_64F) );
CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
(tsize == Size(1, 3) || tsize == Size(3, 1)) );
}
else
{
_rvec.create(3, 1, CV_64F);
_tvec.create(3, 1, CV_64F);
}
rvec = _rvec.getMat();
tvec = _tvec.getMat();
Mat cameraMatrix0 = _cameraMatrix.getMat();
Mat distCoeffs0 = _distCoeffs.getMat();
Mat cameraMatrix = Mat_<double>(cameraMatrix0);
Mat distCoeffs = Mat_<double>(distCoeffs0);
bool result = false;
if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
{
@@ -69,10 +93,10 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
epnp PnP(cameraMatrix, opoints, undistortedPoints);
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
Mat R;
PnP.compute_pose(R, tvec);
Rodrigues(R, rvec);
return true;
result = true;
}
else if (flags == SOLVEPNP_P3P)
{
@@ -81,21 +105,20 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
p3p P3Psolver(cameraMatrix);
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
Mat R;
result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result)
Rodrigues(R, rvec);
return result;
}
else if (flags == SOLVEPNP_ITERATIVE)
{
CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
CvMat c_rvec = rvec, c_tvec = tvec;
cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
&c_rvec, &c_tvec, useExtrinsicGuess );
return true;
result = true;
}
/*else if (flags == SOLVEPNP_DLS)
{
@@ -115,17 +138,13 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
upnp PnP(cameraMatrix, opoints, ipoints);
Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
double f = PnP.compute_pose(R, tvec);
PnP.compute_pose(R, tvec);
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;
return result;
}
class PnPRansacCallback : public PointSetRegistrator::Callback
@@ -196,7 +215,16 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
OutputArray _inliers, int flags)
{
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
Mat opoints, ipoints;
if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
opoints0.convertTo(opoints, CV_32F);
else
opoints = opoints0;
if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
ipoints0.convertTo(ipoints, CV_32F);
else
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)) );