void Marker::calculateExtrinsics(float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff,
bool setYPerpendicular)
{
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__);
vector<cv::Point3f> objpoints = get3DPoints(markerSizeMeters);
cv::Mat raux, taux;
// cv::solvePnP(objpoints, *this, camMatrix, distCoeff, raux, taux);
solvePnP(objpoints, *this,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;
// cout<<(*this)<<endl;
// auto setPrecision=[](double f, double prec){
// int x=roundf(f*prec);
// return double(x)/prec;
// };
// for(int i=0;i<3;i++){
// Rvec.ptr<float>(0)[i]=setPrecision(Rvec.ptr<float>(0)[i],100);
// Tvec.ptr<float>(0)[i]=setPrecision(Tvec.ptr<float>(0)[i],1000);
// }
}
void Camera::addMouseLookDelta( float deltaX, float deltaY )
{
if ( deltaY != 0.0f )
{
rotateYAxis( deltaY );
}
if ( deltaX != 0.0f )
{
rotateXAxis( deltaX );
}
}
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;
}
float BoardDetector::detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix, Mat distCoeff,
float markerSizeMeters) throw(cv::Exception) {
if (BConf.size() == 0)
throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty", __FILE__, __LINE__);
if (BConf[0].size() < 2)
throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty 2", __FILE__, __LINE__);
// compute the size of the markers in meters, which is used for some routines(mostly drawing)
float ssize;
if (BConf.mInfoType == BoardConfiguration::PIX && markerSizeMeters > 0)
ssize = markerSizeMeters;
else if (BConf.mInfoType == BoardConfiguration::METERS) {
ssize = cv::norm(BConf[0][0] - BConf[0][1]);
}
// cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
Bdetected.clear();
/// find among detected markers these that belong to the board configuration
for (unsigned int i = 0; i < detectedMarkers.size(); i++) {
int idx = BConf.getIndexOfMarkerId(detectedMarkers[i].id);
if (idx != -1) {
Bdetected.push_back(detectedMarkers[i]);
Bdetected.back().ssize = ssize;
}
}
// copy configuration
Bdetected.conf = BConf;
//
bool hasEnoughInfoForRTvecCalculation = false;
if (Bdetected.size() >= 1) {
if (camMatrix.rows != 0) {
if (markerSizeMeters > 0 && BConf.mInfoType == BoardConfiguration::PIX)
hasEnoughInfoForRTvecCalculation = true;
else if (BConf.mInfoType == BoardConfiguration::METERS)
hasEnoughInfoForRTvecCalculation = true;
}
}
// calculate extrinsic if there is information for that
if (hasEnoughInfoForRTvecCalculation) {
// calculate the size of the markers in meters if expressed in pixels
double marker_meter_per_pix = 0;
if (BConf.mInfoType == BoardConfiguration::PIX)
marker_meter_per_pix = markerSizeMeters / cv::norm(BConf[0][0] - BConf[0][1]);
else
marker_meter_per_pix = 1; // to avoind interferring the process below
// now, create the matrices for finding the extrinsics
vector< cv::Point3f > objPoints;
vector< cv::Point2f > imagePoints;
for (size_t i = 0; i < Bdetected.size(); i++) {
int idx = Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
assert(idx != -1);
for (int p = 0; p < 4; p++) {
imagePoints.push_back(Bdetected[i][p]);
const aruco::MarkerInfo &Minfo = Bdetected.conf.getMarkerInfo(Bdetected[i].id);
objPoints.push_back(Minfo[p] * marker_meter_per_pix);
// cout<<objPoints.back()<<endl;
}
}
if (distCoeff.total() == 0)
distCoeff = cv::Mat::zeros(1, 4, CV_32FC1);
// for(size_t i=0;i< imagePoints.size();i++){
// cout<<objPoints[i]<<" "<<imagePoints[i]<<endl;
// }
// cout<<"cam="<<camMatrix<<" "<<distCoeff<<endl;
cv::Mat rvec, tvec;
cv::solvePnP(objPoints, imagePoints, camMatrix, distCoeff, rvec, tvec);
rvec.convertTo(Bdetected.Rvec, CV_32FC1);
tvec.convertTo(Bdetected.Tvec, CV_32FC1);
// cout<<rvec<< " "<<tvec<<" _setYPerpendicular="<<_setYPerpendicular<<endl;
{
vector< cv::Point2f > reprojected;
cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);
double errSum = 0;
// check now the reprojection error and
for (size_t i = 0; i < reprojected.size(); i++) {
errSum += cv::norm(reprojected[i] - imagePoints[i]);
}
// cout<<"AAA RE="<<errSum/double ( reprojected.size() ) <<endl;
}
// now, do a refinement and remove points whose reprojection error is above a threshold, then repeat calculation with the rest
if (repj_err_thres > 0) {
vector< cv::Point2f > reprojected;
cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);
vector< int > pointsThatPassTest; // indices
// check now the reprojection error and
for (size_t i = 0; i < reprojected.size(); i++) {
float err = cv::norm(reprojected[i] - imagePoints[i]);
if (err < repj_err_thres)
pointsThatPassTest.push_back(i);
}
// cerr<<"Number of points after reprjection test "<<pointsThatPassTest.size() <<"/"<<objPoints.size() <<endl;
// copy these data to another vectors and repeat
vector< cv::Point3f > objPoints_filtered;
vector< cv::Point2f > imagePoints_filtered;
for (size_t i = 0; i < pointsThatPassTest.size(); i++) {
objPoints_filtered.push_back(objPoints[pointsThatPassTest[i]]);
imagePoints_filtered.push_back(imagePoints[pointsThatPassTest[i]]);
}
cv::solvePnP(objPoints_filtered, imagePoints_filtered, camMatrix, distCoeff, rvec, tvec);
rvec.convertTo(Bdetected.Rvec, CV_32FC1);
tvec.convertTo(Bdetected.Tvec, CV_32FC1);
}
// now, rotate 90 deg in X so that Y axis points up
if (_setYPerpendicular)
rotateXAxis(Bdetected.Rvec);
// cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
// cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
}
float prob = float(Bdetected.size()) / double(Bdetected.conf.size());
return prob;
}
float BoardDetector::detect(const vector<Marker> &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters)throw (cv::Exception)
{
// cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
///find among detected markers these that belong to the board configuration
Mat detected(BConf._markersId.size(),CV_32SC1); //stores the indices of the makers
detected.setTo(Scalar(-1));//-1 mean not detected
int nMarkInBoard=0;//total number of markers detected
for (unsigned int i=0;i<detectedMarkers.size();i++) {
bool found=false;
int id=detectedMarkers[i].id;
//find it
for ( int j=0;j<detected.size().height && ! found;j++)
for ( int k=0;k<detected.size().width && ! found;k++)
if ( BConf._markersId.at<int>(j,k)==id) {
detected.at<int>(j,k)=i;
nMarkInBoard++;
found=true;
Bdetected.push_back(detectedMarkers[i]);
if (markerSizeMeters>0)
Bdetected.back().ssize=markerSizeMeters;
}
}
Bdetected.conf=BConf;
if (markerSizeMeters!=-1)
Bdetected.markerSizeMeters=markerSizeMeters;
//calculate extrinsic if there is information for that
if (camMatrix.rows!=0 && markerSizeMeters>0 && detectedMarkers.size()>1) {
// now, create the matrices for finding the extrinsics
Mat objPoints(4*nMarkInBoard,3,CV_32FC1);
Mat imagePoints(4*nMarkInBoard,2,CV_32FC1);
//size in meters of inter-marker distance
double markerDistanceMeters= double(BConf._markerDistancePix) * markerSizeMeters / double(BConf._markerSizePix);
int currIndex=0;
for ( int y=0;y<detected.size().height;y++)
for ( int x=0;x<detected.size().width;x++) {
if ( detected.at<int>(y,x)!=-1 ) {
vector<Point2f> points =detectedMarkers[ detected.at<int>(y,x) ];
//set first image points
for (int p=0;p<4;p++) {
imagePoints.at<float>(currIndex+p,0)=points[p].x;
imagePoints.at<float>(currIndex+p,1)=points[p].y;
}
//tranaltion to make the Ref System be in center
float TX=-( ((detected.size().height-1)*(markerDistanceMeters+markerSizeMeters) +markerSizeMeters) /2) ;
float TY=-( ((detected.size().width-1)*(markerDistanceMeters+markerSizeMeters) +markerSizeMeters)/2);
//points in real refernce system. We se the center in the bottom-left corner
float AY=x*(markerDistanceMeters+markerSizeMeters ) +TY;
float AX=y*(markerDistanceMeters+markerSizeMeters )+TX;
objPoints.at<float>( currIndex,0)= AX;
objPoints.at<float>( currIndex,1)= AY;
objPoints.at<float>( currIndex,2)= 0;
objPoints.at<float>( currIndex+1,0)= AX;
objPoints.at<float>( currIndex+1,1)= AY+markerSizeMeters;
objPoints.at<float>( currIndex+1,2)= 0;
objPoints.at<float>( currIndex+2,0)= AX+markerSizeMeters;
objPoints.at<float>( currIndex+2,1)= AY+markerSizeMeters;
objPoints.at<float>( currIndex+2,2)= 0;
objPoints.at<float>( currIndex+3,0)= AX+markerSizeMeters;
objPoints.at<float>( currIndex+3,1)= AY;
objPoints.at<float>( currIndex+3,2)= 0;
currIndex+=4;
}
}
CvMat cvCamMatrix=camMatrix;
CvMat cvDistCoeffs;
Mat zeros=Mat::zeros(4,1,CV_32FC1);
if (distCoeff.rows>=4) cvDistCoeffs=distCoeff;
else cvDistCoeffs=zeros;
CvMat cvImgPoints=imagePoints;
CvMat cvObjPoints=objPoints;
CvMat cvRvec=Bdetected.Rvec;
CvMat cvTvec=Bdetected.Tvec;
cvFindExtrinsicCameraParams2(&cvObjPoints, &cvImgPoints, &cvCamMatrix, &cvDistCoeffs,&cvRvec,&cvTvec);
//now, rotate 90 deg in X so that Y axis points up
rotateXAxis(Bdetected.Rvec);
//cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<Bdetected.Rvec.at<float>(2,0)<<endl;
//cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<Bdetected.Tvec.at<float>(2,0)<<endl;
}
return double(nMarkInBoard)/double( BConf._markersId.size().width*BConf._markersId.size().height);
}
float BoardDetector::detect ( const vector<Marker> &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters ) throw ( cv::Exception )
{
if (BConf.size()==0) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty",__FILE__,__LINE__);
if (BConf[0].size()<2) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty 2",__FILE__,__LINE__);
//compute the size of the markers in meters, which is used for some routines(mostly drawing)
float ssize;
if ( BConf.mInfoType==BoardConfiguration::PIX && markerSizeMeters>0 ) ssize=markerSizeMeters;
else if ( BConf.mInfoType==BoardConfiguration::METERS )
{
ssize=cv::norm ( BConf[0][0]-BConf[0][1] );
}
// cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
Bdetected.clear();
///find among detected markers these that belong to the board configuration
for ( unsigned int i=0;i<detectedMarkers.size();i++ )
{
int idx=BConf.getIndexOfMarkerId(detectedMarkers[i].id);
if (idx!=-1) {
Bdetected.push_back ( detectedMarkers[i] );
Bdetected.back().ssize=ssize;
}
}
//copy configuration
Bdetected.conf=BConf;
//
bool hasEnoughInfoForRTvecCalculation=false;
if ( Bdetected.size() >=1 )
{
if ( camMatrix.rows!=0 )
{
if ( markerSizeMeters>0 && BConf.mInfoType==BoardConfiguration::PIX ) hasEnoughInfoForRTvecCalculation=true;
else if ( BConf.mInfoType==BoardConfiguration::METERS ) hasEnoughInfoForRTvecCalculation=true;
}
}
//calculate extrinsic if there is information for that
if ( hasEnoughInfoForRTvecCalculation )
{
//calculate the size of the markers in meters if expressed in pixels
double marker_meter_per_pix=0;
if ( BConf.mInfoType==BoardConfiguration::PIX ) marker_meter_per_pix=markerSizeMeters / cv::norm ( BConf[0][0]-BConf[0][1] );
else marker_meter_per_pix=1;//to avoind interferring the process below
// now, create the matrices for finding the extrinsics
Mat objPoints ( 4*Bdetected.size(),3,CV_32FC1 );
Mat imagePoints ( 4*Bdetected.size(),2,CV_32FC1 );
//size in meters of inter-marker distance
for ( size_t i=0;i<Bdetected.size();i++ )
{
int idx=Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
assert(idx!=-1);
for ( int p=0;p<4;p++ )
{
imagePoints.at<float> ( ( i*4 ) +p,0 ) =Bdetected[i][p].x;
imagePoints.at<float> ( ( i*4 ) +p,1 ) =Bdetected[i][p].y;
const aruco::MarkerInfo &Minfo=Bdetected.conf.getMarkerInfo( Bdetected[i].id);
objPoints.at<float> ( ( i*4 ) +p,0 ) = Minfo[p].x*marker_meter_per_pix;
objPoints.at<float> ( ( i*4 ) +p,1 ) = Minfo[p].y*marker_meter_per_pix;
objPoints.at<float> ( ( i*4 ) +p,2 ) = Minfo[p].z*marker_meter_per_pix;
// cout<<objPoints.at<float>( (i*4)+p,0)<<" "<<objPoints.at<float>( (i*4)+p,1)<<" "<<objPoints.at<float>( (i*4)+p,2)<<endl;
}
}
if (distCoeff.total()==0) distCoeff=cv::Mat::zeros(1,4,CV_32FC1 );
cv::Mat rvec,tvec;
cv::solvePnP(objPoints,imagePoints,camMatrix,distCoeff,rvec,tvec );
rvec.convertTo(Bdetected.Rvec,CV_32FC1);
tvec.convertTo(Bdetected.Tvec,CV_32FC1);
//now, rotate 90 deg in X so that Y axis points up
if (_setYPerperdicular)
rotateXAxis ( Bdetected.Rvec );
// cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
// cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
}
float prob=float( Bdetected.size() ) /double ( Bdetected.conf.size() );
return prob;
}
