mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-11-26 20:20:20 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fix useExtrinsicGuess in solvePnP

Browse Source
This commit is contained in:
S. Garrido 2016-06-09 17:34:00 +01:00 committed by Sergio Garrido
parent 77849d0033
commit e5d12502c5
1 changed files with 46 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,17 +53,36 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat(); Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); 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 >= 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 = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
Mat rvec, tvec;
if( flags != CV_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();
if (flags == CV_EPNP) if (flags == CV_EPNP)
{ {
cv::Mat undistortedPoints; cv::Mat undistortedPoints;
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
epnp PnP(cameraMatrix, opoints, undistortedPoints); epnp PnP(cameraMatrix, opoints, undistortedPoints);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); cv::Mat R;
PnP.compute_pose(R, tvec); PnP.compute_pose(R, tvec);
cv::Rodrigues(R, rvec); cv::Rodrigues(R, rvec);
return true; return true;
@ -75,7 +94,7 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs); cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
p3p P3Psolver(cameraMatrix); p3p P3Psolver(cameraMatrix);
cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat(); cv::Mat R;
bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints); bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
if (result) if (result)
cv::Rodrigues(R, rvec); cv::Rodrigues(R, rvec);
@ -85,7 +104,7 @@ bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
{ {
CvMat c_objectPoints = opoints, c_imagePoints = ipoints; CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs; 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, cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0, c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
&c_rvec, &c_tvec, useExtrinsicGuess ); &c_rvec, &c_tvec, useExtrinsicGuess );
@ -316,10 +335,26 @@ 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);
_rvec.create(3, 1, CV_64FC1); Mat rvec, tvec;
_tvec.create(3, 1, CV_64FC1); if( flags != CV_ITERATIVE )
Mat rvec = _rvec.getMat(); useExtrinsicGuess = false;
Mat tvec = _tvec.getMat();
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 objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1); Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
@ -360,7 +395,8 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1)); Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints); objectPoints.col(index).copyTo(colInlierObjectPoints);
} }
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags); solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion,
localRvec, localTvec, params.useExtrinsicGuess, flags);
} }
localRvec.copyTo(rvec); localRvec.copyTo(rvec);
localTvec.copyTo(tvec); localTvec.copyTo(tvec);