00001
00030 #include "arucofidmarkers.h"
00031 #include <opencv2/imgproc/imgproc.hpp>
00032 using namespace cv;
00033 using namespace std;
00034 namespace aruco {
00035
00036
00037
00038
00039
00040
00041
00044 Mat FiducidalMarkers::createMarkerImage(int id,int size) throw (cv::Exception)
00045 {
00046 Mat marker(size,size, CV_8UC1);
00047 marker.setTo(Scalar(0));
00048 if (0<=id && id<1024) {
00049
00050 int swidth=size/7;
00051 int ids[4]={0x10,0x17,0x09,0x0e};
00052 for (int y=0;y<5;y++) {
00053 int index=(id>>2*(4-y)) & 0x0003;
00054 int val=ids[index];
00055 for (int x=0;x<5;x++) {
00056 Mat roi=marker(Rect((x+1)* swidth,(y+1)* swidth,swidth,swidth));
00057 if ( ( val>>(4-x) ) & 0x0001 ) roi.setTo(Scalar(255));
00058 else roi.setTo(Scalar(0));
00059 }
00060 }
00061 }
00062 else throw cv::Exception(9004,"id invalid","createMarker",__FILE__,__LINE__);
00063
00064 return marker;
00065 }
00069 cv::Mat FiducidalMarkers::getMarkerMat(int id) throw (cv::Exception)
00070 {
00071 Mat marker(5,5, CV_8UC1);
00072 marker.setTo(Scalar(0));
00073 if (0<=id && id<1024) {
00074
00075 int ids[4]={0x10,0x17,0x09,0x0e};
00076 for (int y=0;y<5;y++) {
00077 int index=(id>>2*(4-y)) & 0x0003;
00078 int val=ids[index];
00079 for (int x=0;x<5;x++) {
00080 if ( ( val>>(4-x) ) & 0x0001 ) marker.at<uchar>(y,x)=1;
00081 else marker.at<uchar>(y,x)=0;
00082 }
00083 }
00084 }
00085 else throw cv::Exception (9189,"Invalid marker id","aruco::fiducidal::createMarkerMat",__FILE__,__LINE__);
00086 return marker;
00087 }
00088
00089
00090
00091
00092
00093
00094
00095 cv::Mat FiducidalMarkers::createBoardImage( Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,vector<int> *excludedIds) throw (cv::Exception)
00096 {
00097
00098
00099
00100 srand(cv::getTickCount());
00101 int nMarkers=gridSize.height*gridSize.width;
00102 TInfo.resize(nMarkers);
00103 vector<int> ids=getListOfValidMarkersIds_random(nMarkers,excludedIds);
00104 for (int i=0;i<nMarkers;i++)
00105 TInfo[i].id=ids[i];
00106
00107 int sizeY=gridSize.height*MarkerSize+(gridSize.height-1)*MarkerDistance;
00108 int sizeX=gridSize.width*MarkerSize+(gridSize.width-1)*MarkerDistance;
00109
00110 int centerX=sizeX/2;
00111 int centerY=sizeY/2;
00112
00113
00114 TInfo.mInfoType=BoardConfiguration::PIX;
00115 Mat tableImage(sizeY,sizeX,CV_8UC1);
00116 tableImage.setTo(Scalar(255));
00117 int idp=0;
00118 for (int y=0;y<gridSize.height;y++)
00119 for (int x=0;x<gridSize.width;x++,idp++) {
00120 Mat subrect(tableImage,Rect( x*(MarkerDistance+MarkerSize),y*(MarkerDistance+MarkerSize),MarkerSize,MarkerSize));
00121 Mat marker=createMarkerImage( TInfo[idp].id,MarkerSize);
00122
00123 TInfo[idp].resize(4);
00124 TInfo[idp][0]=cv::Point3f( x*(MarkerDistance+MarkerSize),y*(MarkerDistance+MarkerSize),0);
00125 TInfo[idp][1]=cv::Point3f( x*(MarkerDistance+MarkerSize)+MarkerSize,y*(MarkerDistance+MarkerSize),0);
00126 TInfo[idp][2]=cv::Point3f( x*(MarkerDistance+MarkerSize)+MarkerSize,y*(MarkerDistance+MarkerSize)+MarkerSize,0);
00127 TInfo[idp][3]=cv::Point3f( x*(MarkerDistance+MarkerSize),y*(MarkerDistance+MarkerSize)+MarkerSize,0);
00128 for (int i=0;i<4;i++) TInfo[idp][i]-=cv::Point3f(centerX,centerY,0);
00129 marker.copyTo(subrect);
00130 }
00131
00132 return tableImage;
00133 }
00134
00135
00136
00137
00138
00139
00140
00141 cv::Mat FiducidalMarkers::createBoardImage_ChessBoard( Size gridSize,int MarkerSize, BoardConfiguration& TInfo ,bool centerData ,vector<int> *excludedIds) throw (cv::Exception)
00142 {
00143
00144
00145 srand(cv::getTickCount());
00146
00147
00148 int nMarkers= 3*(gridSize.width*gridSize.height)/4;
00149 vector<int> idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds);
00150
00151
00152 int sizeY=gridSize.height*MarkerSize;
00153 int sizeX=gridSize.width*MarkerSize;
00154
00155 int centerX=sizeX/2;
00156 int centerY=sizeY/2;
00157
00158 Mat tableImage(sizeY,sizeX,CV_8UC1);
00159 tableImage.setTo(Scalar(255));
00160 TInfo.mInfoType=BoardConfiguration::PIX;
00161 int CurMarkerIdx=0;
00162 for (int y=0;y<gridSize.height;y++) {
00163
00164 bool toWrite;
00165 if (y%2==0) toWrite=false;
00166 else toWrite=true;
00167 for (int x=0;x<gridSize.width;x++) {
00168 toWrite=!toWrite;
00169 if (toWrite) {
00170 if (CurMarkerIdx>=idsVector.size()) throw cv::Exception(999," FiducidalMarkers::createBoardImage_ChessBoard","INTERNAL ERROR. REWRITE THIS!!",__FILE__,__LINE__);
00171 TInfo.push_back( MarkerInfo(idsVector[CurMarkerIdx++]));
00172
00173 Mat subrect(tableImage,Rect( x*MarkerSize,y*MarkerSize,MarkerSize,MarkerSize));
00174 Mat marker=createMarkerImage( TInfo.back().id,MarkerSize);
00175
00176 TInfo.back().resize(4);
00177 TInfo.back()[0]=cv::Point3f( x*(MarkerSize),y*(MarkerSize),0);
00178 TInfo.back()[1]=cv::Point3f( x*(MarkerSize)+MarkerSize,y*(MarkerSize),0);
00179 TInfo.back()[2]=cv::Point3f( x*(MarkerSize)+MarkerSize,y*(MarkerSize)+MarkerSize,0);
00180 TInfo.back()[3]=cv::Point3f( x*(MarkerSize),y*(MarkerSize)+MarkerSize,0);
00181 if (centerData) {
00182 for (int i=0;i<4;i++)
00183 TInfo.back()[i]-=cv::Point3f(centerX,centerY,0);
00184 }
00185 marker.copyTo(subrect);
00186 }
00187 }
00188 }
00189
00190 return tableImage;
00191 }
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201 cv::Mat FiducidalMarkers::createBoardImage_Frame( Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,bool centerData,vector<int> *excludedIds ) throw (cv::Exception)
00202 {
00203
00204
00205
00206 srand(cv::getTickCount());
00207 int nMarkers=2*gridSize.height*2*gridSize.width;
00208 vector<int> idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds);
00209
00210 int sizeY=gridSize.height*MarkerSize+MarkerDistance*(gridSize.height-1);
00211 int sizeX=gridSize.width*MarkerSize+MarkerDistance*(gridSize.width-1);
00212
00213 int centerX=sizeX/2;
00214 int centerY=sizeY/2;
00215
00216 Mat tableImage(sizeY,sizeX,CV_8UC1);
00217 tableImage.setTo(Scalar(255));
00218 TInfo.mInfoType=BoardConfiguration::PIX;
00219 int CurMarkerIdx=0;
00220 int mSize=MarkerSize+MarkerDistance;
00221 for (int y=0;y<gridSize.height;y++) {
00222 for (int x=0;x<gridSize.width;x++) {
00223 if (y==0 || y==gridSize.height-1 || x==0 || x==gridSize.width-1) {
00224 TInfo.push_back( MarkerInfo(idsVector[CurMarkerIdx++]));
00225 Mat subrect(tableImage,Rect( x*mSize,y*mSize,MarkerSize,MarkerSize));
00226 Mat marker=createMarkerImage( TInfo.back().id,MarkerSize);
00227 marker.copyTo(subrect);
00228
00229 TInfo.back().resize(4);
00230 TInfo.back()[0]=cv::Point3f( x*(mSize),y*(mSize),0);
00231 TInfo.back()[1]=cv::Point3f( x*(mSize)+MarkerSize,y*(mSize),0);
00232 TInfo.back()[2]=cv::Point3f( x*(mSize)+MarkerSize,y*(mSize)+MarkerSize,0);
00233 TInfo.back()[3]=cv::Point3f( x*(mSize),y*(mSize)+MarkerSize,0);
00234 if (centerData) {
00235 for (int i=0;i<4;i++)
00236 TInfo.back()[i]-=cv::Point3f(centerX,centerY,0);
00237 }
00238
00239 }
00240 }
00241 }
00242
00243 return tableImage;
00244 }
00245
00246
00247
00248
00249
00250
00251 Mat FiducidalMarkers::rotate(const Mat &in)
00252 {
00253 Mat out;
00254 in.copyTo(out);
00255 for (int i=0;i<in.rows;i++)
00256 {
00257 for (int j=0;j<in.cols;j++)
00258 {
00259 out.at<uchar>(i,j)=in.at<uchar>(in.cols-j-1,i);
00260 }
00261 }
00262 return out;
00263 }
00264
00265
00266
00267
00268
00269
00270
00271
00272 int FiducidalMarkers::hammDistMarker(Mat bits)
00273 {
00274 int ids[4][5]=
00275 {
00276 {
00277 1,0,0,0,0
00278 }
00279 ,
00280 {
00281 1,0,1,1,1
00282 }
00283 ,
00284 {
00285 0,1,0,0,1
00286 }
00287 ,
00288 {
00289 0, 1, 1, 1, 0
00290 }
00291 };
00292 int dist=0;
00293
00294 for (int y=0;y<5;y++)
00295 {
00296 int minSum=1e5;
00297
00298 for (int p=0;p<4;p++)
00299 {
00300 int sum=0;
00301
00302 for (int x=0;x<5;x++)
00303 sum+= bits.at<uchar>(y,x) == ids[p][x]?0:1;
00304 if (minSum>sum) minSum=sum;
00305 }
00306
00307 dist+=minSum;
00308 }
00309
00310 return dist;
00311 }
00312
00313
00314
00315
00316
00317
00318
00319 int FiducidalMarkers::analyzeMarkerImage(Mat &grey,int &nRotations)
00320 {
00321
00322
00323
00324
00325 int swidth=grey.rows/7;
00326 for (int y=0;y<7;y++)
00327 {
00328 int inc=6;
00329 if (y==0 || y==6) inc=1;
00330 for (int x=0;x<7;x+=inc)
00331 {
00332 int Xstart=(x)*(swidth);
00333 int Ystart=(y)*(swidth);
00334 Mat square=grey(Rect(Xstart,Ystart,swidth,swidth));
00335 int nZ=countNonZero(square);
00336 if (nZ> (swidth*swidth) /2) {
00337
00338 return -1;
00339 }
00340 }
00341 }
00342
00343
00344 vector<int> markerInfo(5);
00345 Mat _bits=Mat::zeros(5,5,CV_8UC1);
00346
00347
00348 for (int y=0;y<5;y++)
00349 {
00350
00351 for (int x=0;x<5;x++)
00352 {
00353 int Xstart=(x+1)*(swidth);
00354 int Ystart=(y+1)*(swidth);
00355 Mat square=grey(Rect(Xstart,Ystart,swidth,swidth));
00356 int nZ=countNonZero(square);
00357 if (nZ> (swidth*swidth) /2) _bits.at<uchar>( y,x)=1;
00358 }
00359 }
00360
00361
00362
00363 Mat _bitsFlip;
00364 Mat Rotations[4];
00365 Rotations[0]=_bits;
00366 int dists[4];
00367 dists[0]=hammDistMarker( Rotations[0]) ;
00368 pair<int,int> minDist( dists[0],0);
00369 for (int i=1;i<4;i++)
00370 {
00371
00372 Rotations[i]=rotate(Rotations[i-1]);
00373
00374 dists[i]=hammDistMarker( Rotations[i]) ;
00375 if (dists[i]<minDist.first)
00376 {
00377 minDist.first= dists[i];
00378 minDist.second=i;
00379 }
00380 }
00381
00382
00383
00384 nRotations=minDist.second;
00385 if (minDist.first!=0)
00386 return -1;
00387 else {
00388 int MatID=0;
00389 cv::Mat bits=Rotations [ minDist.second];
00390 for (int y=0;y<5;y++)
00391 {
00392 MatID<<=1;
00393 if ( bits.at<uchar>(y,1)) MatID|=1;
00394 MatID<<=1;
00395 if ( bits.at<uchar>(y,3)) MatID|=1;
00396 }
00397 return MatID;
00398 }
00399 }
00400
00401
00402
00403
00404
00405
00406
00407
00408 bool FiducidalMarkers::correctHammMarker(Mat &bits)
00409 {
00410
00411 bool errors[4];
00412 int ids[4][5]=
00413 {
00414 {
00415 0,0,0,0,0
00416 }
00417 ,
00418 {
00419 0,0,1,1,1
00420 }
00421 ,
00422 {
00423 1,1,0,0,1
00424 }
00425 ,
00426 {
00427 1, 1, 1, 1, 0
00428 }
00429 };
00430
00431 for (int y=0;y<5;y++)
00432 {
00433 int minSum=1e5;
00434
00435 for (int p=0;p<4;p++)
00436 {
00437 int sum=0;
00438
00439 for (int x=0;x<5;x++)
00440 sum+= bits.at<uchar>(y,x) == ids[p][x]?0:1;
00441 if (minSum>sum) minSum=sum;
00442 }
00443 if (minSum!=0) errors[y]=true;
00444 else errors[y]=false;
00445 }
00446
00447 return true;
00448 }
00449
00450
00451
00452
00453
00454
00455
00456 int FiducidalMarkers::detect(const Mat &in,int &nRotations)
00457 {
00458 assert(in.rows==in.cols);
00459 Mat grey;
00460 if ( in.type()==CV_8UC1) grey=in;
00461 else cv::cvtColor(in,grey,CV_BGR2GRAY);
00462
00463 threshold(grey, grey,125, 255, THRESH_BINARY|THRESH_OTSU);
00464
00465
00466
00467
00468 return analyzeMarkerImage(grey,nRotations);;
00469
00470
00471
00472
00473
00474
00475 }
00476
00477 vector<int> FiducidalMarkers::getListOfValidMarkersIds_random(int nMarkers,vector<int> *excluded) throw (cv::Exception)
00478 {
00479
00480 if (excluded!=NULL)
00481 if (nMarkers+excluded->size()>1024) throw cv::Exception(8888,"FiducidalMarkers::getListOfValidMarkersIds_random","Number of possible markers is exceeded",__FILE__,__LINE__);
00482
00483 vector<int> listOfMarkers(1024);
00484
00485 for (int i=0;i<1024;i++) listOfMarkers[i]=i;
00486
00487 if (excluded!=NULL)
00488 for (size_t i=0;i<excluded->size();i++)
00489 listOfMarkers[excluded->at(i)]=-1;
00490
00491 random_shuffle(listOfMarkers.begin(),listOfMarkers.end());
00492
00493 int i=0;
00494 vector<int> retList;
00495 while (retList.size()<nMarkers) {
00496 if (listOfMarkers[i]!=-1)
00497 retList.push_back(listOfMarkers[i]);
00498 i++;
00499 }
00500 return retList;
00501 }
00502
00503 }
00504