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

fixed useExtrinsicGuess=true case with single-precision input (http://code.opencv.org/issues/2734)

Browse Source
This commit is contained in:
Vadim Pisarevsky 2015-05-25 22:40:10 +03:00
parent 9d90b0549c
commit a12ec66a04
2 changed files with 74 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
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)
{
@ -121,7 +144,7 @@ bool solvePnP( InputArray _opoints, InputArray _ipoints,
}*/
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

40
modules/calib3d/test/test_solvepnp_ransac.cpp
View File

@ -344,3 +344,43 @@ TEST(Calib3d_SolvePnP, double_support)
ASSERT_LE(norm(R, Mat_<double>(RF), NORM_INF), 1e-3);
ASSERT_LE(norm(t, Mat_<double>(tF), NORM_INF), 1e-3);
}
TEST(Calib3d_SolvePnP, translation)
{
Mat cameraIntrinsic = Mat::eye(3,3, CV_32FC1);
vector<float> crvec;
crvec.push_back(0.f);
crvec.push_back(0.f);
crvec.push_back(0.f);
vector<float> ctvec;
ctvec.push_back(100.f);
ctvec.push_back(100.f);
ctvec.push_back(0.f);
vector<Point3f> p3d;
p3d.push_back(Point3f(0,0,0));
p3d.push_back(Point3f(0,0,10));
p3d.push_back(Point3f(0,10,10));
p3d.push_back(Point3f(10,10,10));
p3d.push_back(Point3f(2,5,5));
vector<Point2f> p2d;
projectPoints(p3d, crvec, ctvec, cameraIntrinsic, noArray(), p2d);
Mat rvec;
Mat tvec;
rvec =(Mat_<float>(3,1) << 0, 0, 0);
tvec = (Mat_<float>(3,1) << 100, 100, 0);
solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true);
ASSERT_TRUE(checkRange(rvec));
ASSERT_TRUE(checkRange(tvec));
rvec =(Mat_<double>(3,1) << 0, 0, 0);
tvec = (Mat_<double>(3,1) << 100, 100, 0);
solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true);
ASSERT_TRUE(checkRange(rvec));
ASSERT_TRUE(checkRange(tvec));
solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, false);
ASSERT_TRUE(checkRange(rvec));
ASSERT_TRUE(checkRange(tvec));
}