void Marker::calculateExtrinsics(float markerSizeMeters,cv::Mat camMatrix,cv::Mat distCoeff ,bool setYPerpendicular)throw(cv::Exception)
{
if (!isValid()) throw cv::Exception(9004,"!isValid(): invalid marker. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
if (markerSizeMeters<=0)throw cv::Exception(9004,"markerSize<=0: invalid markerSize","calculateExtrinsics",__FILE__,__LINE__);
if ( camMatrix.rows==0 || camMatrix.cols==0) throw cv::Exception(9004,"CameraMatrix is empty","calculateExtrinsics",__FILE__,__LINE__);
double halfSize=markerSizeMeters/2.;
cv::Mat ObjPoints(4,3,CV_32FC1);
ObjPoints.at<float>(1,0)=-halfSize;
ObjPoints.at<float>(1,1)=halfSize;
ObjPoints.at<float>(1,2)=0;
ObjPoints.at<float>(2,0)=halfSize;
ObjPoints.at<float>(2,1)=halfSize;
ObjPoints.at<float>(2,2)=0;
ObjPoints.at<float>(3,0)=halfSize;
ObjPoints.at<float>(3,1)=-halfSize;
ObjPoints.at<float>(3,2)=0;
ObjPoints.at<float>(0,0)=-halfSize;
ObjPoints.at<float>(0,1)=-halfSize;
ObjPoints.at<float>(0,2)=0;
cv::Mat ImagePoints(4,2,CV_32FC1);
//Set image points from the marker
for (int c=0;c<4;c++)
{
ImagePoints.at<float>(c,0)=((*this)[c].x);
ImagePoints.at<float>(c,1)=((*this)[c].y);
}
cv::Mat raux,taux;
cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux);
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_32F);
//rotate the X axis so that Y is perpendicular to the marker plane
if (setYPerpendicular)
rotateXAxis(Rvec);
ssize=markerSizeMeters;
#ifndef NO_DEBUG_ARUCO
cout<<(*this)<<endl;
#endif
}
void Marker::calculateExtrinsics(float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff,
bool setYPerpendicular) throw(cv::Exception) {
if (!isValid())
throw cv::Exception(9004, "!isValid(): invalid marker. It is not possible to calculate extrinsics",
"calculateExtrinsics", __FILE__, __LINE__);
if (markerSizeMeters <= 0)
throw cv::Exception(9004, "markerSize<=0: invalid markerSize", "calculateExtrinsics", __FILE__,
__LINE__);
if (camMatrix.rows == 0 || camMatrix.cols == 0)
throw cv::Exception(9004, "CameraMatrix is empty", "calculateExtrinsics", __FILE__, __LINE__);
double halfSize = markerSizeMeters / 2.;
cv::Matx<float, 4, 3> ObjPoints;
ObjPoints(1, 0) = -halfSize;
ObjPoints(1, 1) = halfSize;
ObjPoints(1, 2) = 0;
ObjPoints(2, 0) = halfSize;
ObjPoints(2, 1) = halfSize;
ObjPoints(2, 2) = 0;
ObjPoints(3, 0) = halfSize;
ObjPoints(3, 1) = -halfSize;
ObjPoints(3, 2) = 0;
ObjPoints(0, 0) = -halfSize;
ObjPoints(0, 1) = -halfSize;
ObjPoints(0, 2) = 0;
cv::Matx<float, 4, 2> ImagePoints;
// Set image points from the marker
for (int c = 0; c < 4; c++) {
ImagePoints(c, 0) = ((*this)[c].x);
ImagePoints(c, 1) = ((*this)[c].y);
}
cv::Vec3d raux, taux;
cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff, raux, taux);
Rvec = raux;
Tvec = taux;
// rotate the X axis so that Y is perpendicular to the marker plane
if (setYPerpendicular)
rotateXAxis(Rvec);
ssize = markerSizeMeters;
// cout<<(*this)<<endl;
}
