00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "boarddetector.h"
00029 #define _USE_MATH_DEFINES
00030 #include <math.h>
00031 #include <cstdlib>
00032 #include <ctime>
00033 #include <cassert>
00034 #include <fstream>
00035 using namespace std;
00036 using namespace cv;
00037 namespace aruco
00038 {
00041 BoardDetector::BoardDetector(bool setYPerperdicular)
00042 {
00043 _setYPerperdicular=setYPerperdicular;
00044 _areParamsSet=false;
00045 }
00049 void BoardDetector::setParams(const BoardConfiguration &bc,const CameraParameters &cp, float markerSizeMeters) {
00050 _camParams=cp;
00051 _markerSize=markerSizeMeters;
00052 _bconf=bc;
00053 _areParamsSet=true;
00054 }
00059 void BoardDetector::setParams(const BoardConfiguration &bc) {
00060 _bconf=bc;
00061 _areParamsSet=true;
00062 }
00063
00068 float BoardDetector::detect(const cv::Mat &im)throw (cv::Exception)
00069 {
00070 _mdetector.detect(im,_vmarkers);
00071
00072 float res;
00073
00074 if (_camParams.isValid())
00075 res=detect(_vmarkers,_bconf,_boardDetected,_camParams.CameraMatrix,_camParams.Distorsion,_markerSize);
00076 else res=detect(_vmarkers,_bconf,_boardDetected);
00077 return res;
00078 }
00083 float BoardDetector::detect ( const vector<Marker> &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected,const CameraParameters &cp, float markerSizeMeters ) throw ( cv::Exception )
00084 {
00085 return detect ( detectedMarkers, BConf,Bdetected,cp.CameraMatrix,cp.Distorsion,markerSizeMeters );
00086 }
00091 float BoardDetector::detect ( const vector<Marker> &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters ) throw ( cv::Exception )
00092 {
00093 if (BConf.size()==0) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty",__FILE__,__LINE__);
00094 if (BConf[0].size()<2) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty 2",__FILE__,__LINE__);
00095
00096 float ssize;
00097 if ( BConf.mInfoType==BoardConfiguration::PIX && markerSizeMeters>0 ) ssize=markerSizeMeters;
00098 else if ( BConf.mInfoType==BoardConfiguration::METERS )
00099 {
00100 ssize=cv::norm ( BConf[0][0]-BConf[0][1] );
00101 }
00102
00103
00104 Bdetected.clear();
00106 for ( unsigned int i=0;i<detectedMarkers.size();i++ )
00107 {
00108 int idx=BConf.getIndexOfMarkerId(detectedMarkers[i].id);
00109 if (idx!=-1) {
00110 Bdetected.push_back ( detectedMarkers[i] );
00111 Bdetected.back().ssize=ssize;
00112 }
00113 }
00114
00115 Bdetected.conf=BConf;
00116
00117
00118 bool hasEnoughInfoForRTvecCalculation=false;
00119 if ( Bdetected.size() >=1 )
00120 {
00121 if ( camMatrix.rows!=0 )
00122 {
00123 if ( markerSizeMeters>0 && BConf.mInfoType==BoardConfiguration::PIX ) hasEnoughInfoForRTvecCalculation=true;
00124 else if ( BConf.mInfoType==BoardConfiguration::METERS ) hasEnoughInfoForRTvecCalculation=true;
00125 }
00126 }
00127
00128
00129 if ( hasEnoughInfoForRTvecCalculation )
00130 {
00131
00132
00133 double marker_meter_per_pix=0;
00134 if ( BConf.mInfoType==BoardConfiguration::PIX ) marker_meter_per_pix=markerSizeMeters / cv::norm ( BConf[0][0]-BConf[0][1] );
00135 else marker_meter_per_pix=1;
00136
00137
00138 Mat objPoints ( 4*Bdetected.size(),3,CV_32FC1 );
00139 Mat imagePoints ( 4*Bdetected.size(),2,CV_32FC1 );
00140
00141
00142 for ( size_t i=0;i<Bdetected.size();i++ )
00143 {
00144 int idx=Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
00145 assert(idx!=-1);
00146 for ( int p=0;p<4;p++ )
00147 {
00148 imagePoints.at<float> ( ( i*4 ) +p,0 ) =Bdetected[i][p].x;
00149 imagePoints.at<float> ( ( i*4 ) +p,1 ) =Bdetected[i][p].y;
00150 const aruco::MarkerInfo &Minfo=Bdetected.conf.getMarkerInfo( Bdetected[i].id);
00151
00152 objPoints.at<float> ( ( i*4 ) +p,0 ) = Minfo[p].x*marker_meter_per_pix;
00153 objPoints.at<float> ( ( i*4 ) +p,1 ) = Minfo[p].y*marker_meter_per_pix;
00154 objPoints.at<float> ( ( i*4 ) +p,2 ) = Minfo[p].z*marker_meter_per_pix;
00155
00156 }
00157 }
00158 if (distCoeff.total()==0) distCoeff=cv::Mat::zeros(1,4,CV_32FC1 );
00159
00160 cv::Mat rvec,tvec;
00161 cv::solvePnP(objPoints,imagePoints,camMatrix,distCoeff,rvec,tvec );
00162 rvec.convertTo(Bdetected.Rvec,CV_32FC1);
00163 tvec.convertTo(Bdetected.Tvec,CV_32FC1);
00164
00165 if (_setYPerperdicular)
00166 rotateXAxis ( Bdetected.Rvec );
00167 cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
00168 cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
00169 }
00170
00171 float prob=float( Bdetected.size() ) /double ( Bdetected.conf.size() );
00172 return prob;
00173 }
00174
00175 void BoardDetector::rotateXAxis ( Mat &rotation )
00176 {
00177 cv::Mat R ( 3,3,CV_32FC1 );
00178 Rodrigues ( rotation, R );
00179
00180 cv::Mat RX=cv::Mat::eye ( 3,3,CV_32FC1 );
00181 float angleRad=-M_PI/2;
00182 RX.at<float> ( 1,1 ) =cos ( angleRad );
00183 RX.at<float> ( 1,2 ) =-sin ( angleRad );
00184 RX.at<float> ( 2,1 ) =sin ( angleRad );
00185 RX.at<float> ( 2,2 ) =cos ( angleRad );
00186
00187 R=R*RX;
00188
00189 Rodrigues ( R,rotation );
00190
00191 }
00192
00193
00194 };
00195