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)
{
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)
{
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)
{
}*/
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
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));
+}