1 #pragma GCC diagnostic warning "-Wuninitialized" 29 #include <aruco/boarddetector.h> 30 #define _USE_MATH_DEFINES 41 BoardDetector::BoardDetector(
bool setYPerpendicular)
42 : _setYPerpendicular(setYPerpendicular),
49 void BoardDetector::setParams(
const BoardConfiguration &bc,
const CameraParameters &cp,
float markerSizeMeters)
52 _markerSize = markerSizeMeters;
57 void BoardDetector::setParams(
const BoardConfiguration &bc)
64 float BoardDetector::detect(
const cv::Mat &im)
throw (cv::Exception)
66 _mdetector.detect(im,_vmarkers);
70 if (_camParams.isValid())
71 res=detect(_vmarkers,_bconf,_boardDetected,_camParams.CameraMatrix,_camParams.Distorsion,_markerSize);
72 else res=detect(_vmarkers,_bconf,_boardDetected);
76 float BoardDetector::detect (
const vector<Marker> &detectedMarkers,
const BoardConfiguration &BConf, Board &Bdetected,
const CameraParameters &cp,
float markerSizeMeters )
throw ( cv::Exception )
78 return detect ( detectedMarkers, BConf,Bdetected,cp.CameraMatrix,cp.Distorsion,markerSizeMeters );
81 float BoardDetector::detect (
const vector<Marker> &detectedMarkers,
const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,
float markerSizeMeters )
throw ( cv::Exception )
83 if (BConf.size()==0)
throw cv::Exception(8881,
"BoardDetector::detect",
"Invalid BoardConfig that is empty",__FILE__,__LINE__);
84 if (BConf[0].size()<2)
throw cv::Exception(8881,
"BoardDetector::detect",
"Invalid BoardConfig that is empty 2",__FILE__,__LINE__);
87 if ( BConf.mInfoType==BoardConfiguration::PIX && markerSizeMeters>0 ) ssize=markerSizeMeters;
88 else if ( BConf.mInfoType==BoardConfiguration::METERS )
90 ssize=cv::norm ( BConf[0][0]-BConf[0][1] );
96 for (
unsigned int i=0;i<detectedMarkers.size();i++ )
98 int idx=BConf.getIndexOfMarkerId(detectedMarkers[i].
id);
100 Bdetected.push_back ( detectedMarkers[i] );
101 Bdetected.back().ssize=ssize;
105 Bdetected.conf=BConf;
108 bool hasEnoughInfoForRTvecCalculation=
false;
109 if ( Bdetected.size() >=1 )
111 if ( camMatrix.rows!=0 )
113 if ( markerSizeMeters>0 && BConf.mInfoType==BoardConfiguration::PIX ) hasEnoughInfoForRTvecCalculation=
true;
114 else if ( BConf.mInfoType==BoardConfiguration::METERS ) hasEnoughInfoForRTvecCalculation=
true;
119 if ( hasEnoughInfoForRTvecCalculation )
122 double marker_meter_per_pix=0;
123 if ( BConf.mInfoType==BoardConfiguration::PIX ) marker_meter_per_pix=markerSizeMeters / cv::norm ( BConf[0][0]-BConf[0][1] );
124 else marker_meter_per_pix=1;
127 Mat objPoints ( 4*Bdetected.size(),3,CV_32FC1 );
128 Mat imagePoints ( 4*Bdetected.size(),2,CV_32FC1 );
131 for (
size_t i=0;i<Bdetected.size();i++ )
133 int idx=Bdetected.conf.getIndexOfMarkerId(Bdetected[i].
id);
135 for (
int p=0;p<4;p++ )
137 imagePoints.at<
float> ( ( i*4 ) +p,0 ) =Bdetected[i][p].x;
138 imagePoints.at<
float> ( ( i*4 ) +p,1 ) =Bdetected[i][p].y;
141 objPoints.at<
float> ( ( i*4 ) +p,0 ) = Minfo[p].x*marker_meter_per_pix;
142 objPoints.at<
float> ( ( i*4 ) +p,1 ) = Minfo[p].y*marker_meter_per_pix;
143 objPoints.at<
float> ( ( i*4 ) +p,2 ) = Minfo[p].z*marker_meter_per_pix;
147 if (distCoeff.total()==0) distCoeff=cv::Mat::zeros(1,4,CV_32FC1 );
150 cv::solvePnP(objPoints,imagePoints,camMatrix,distCoeff,rvec,tvec );
151 rvec.convertTo(Bdetected.Rvec,CV_32FC1);
152 tvec.convertTo(Bdetected.Tvec,CV_32FC1);
154 if (_setYPerpendicular)
155 rotateXAxis ( Bdetected.Rvec );
160 float prob=float( Bdetected.size() ) /
double ( Bdetected.conf.size() );
164 void BoardDetector::rotateXAxis ( Mat &rotation )
166 cv::Mat R ( 3,3,CV_32FC1 );
167 Rodrigues ( rotation, R );
169 cv::Mat RX=cv::Mat::eye ( 3,3,CV_32FC1 );
170 float angleRad=-M_PI/2;
171 RX.at<
float> ( 1,1 ) =cos ( angleRad );
172 RX.at<
float> ( 1,2 ) =-sin ( angleRad );
173 RX.at<
float> ( 2,1 ) =sin ( angleRad );
174 RX.at<
float> ( 2,2 ) =cos ( angleRad );
178 Rodrigues ( R,rotation );