arucofidmarkers.cpp
Go to the documentation of this file.
00001 
00030 #include <aruco/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       //for each line, create
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       //for each line, create
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     //find the center so that the ref systeem is in it
00110     int centerX=sizeX/2;
00111     int centerY=sizeY/2;
00112 
00113     //indicate the data is expressed in pixels
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         //set the location of the corners
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     //determine the total number of markers required
00148     int nMarkers= 3*(gridSize.width*gridSize.height)/4;//overdetermine  the number of marker read
00149     vector<int> idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds);
00150 
00151 
00152     int sizeY=gridSize.height*MarkerSize;
00153     int sizeX=gridSize.width*MarkerSize;
00154     //find the center so that the ref systeem is in it
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     unsigned 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           //set the location of the corners
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     srand(cv::getTickCount());
00204     int nMarkers=2*gridSize.height*2*gridSize.width;
00205     vector<int> idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds);
00206 
00207     int sizeY=gridSize.height*MarkerSize+MarkerDistance*(gridSize.height-1);
00208     int sizeX=gridSize.width*MarkerSize+MarkerDistance*(gridSize.width-1);
00209     //find the center so that the ref systeem is in it
00210     int centerX=sizeX/2;
00211     int centerY=sizeY/2;
00212 
00213     Mat tableImage(sizeY,sizeX,CV_8UC1);
00214     tableImage.setTo(Scalar(255));
00215     TInfo.mInfoType=BoardConfiguration::PIX;
00216     int CurMarkerIdx=0;
00217     int mSize=MarkerSize+MarkerDistance;
00218     for (int y=0;y<gridSize.height;y++) {
00219       for (int x=0;x<gridSize.width;x++) {
00220         if (y==0 || y==gridSize.height-1 || x==0 ||  x==gridSize.width-1) {
00221           TInfo.push_back(  MarkerInfo(idsVector[CurMarkerIdx++]));
00222           Mat subrect(tableImage,Rect( x*mSize,y*mSize,MarkerSize,MarkerSize));
00223           Mat marker=createMarkerImage( TInfo.back().id,MarkerSize);
00224           marker.copyTo(subrect);
00225           //set the location of the corners
00226           TInfo.back().resize(4);
00227           TInfo.back()[0]=cv::Point3f( x*(mSize),y*(mSize),0);
00228           TInfo.back()[1]=cv::Point3f( x*(mSize)+MarkerSize,y*(mSize),0);
00229           TInfo.back()[2]=cv::Point3f( x*(mSize)+MarkerSize,y*(mSize)+MarkerSize,0);
00230           TInfo.back()[3]=cv::Point3f( x*(mSize),y*(mSize)+MarkerSize,0);
00231           if (centerData) {
00232             for (int i=0;i<4;i++)
00233               TInfo.back()[i]-=cv::Point3f(centerX,centerY,0);
00234           }
00235 
00236         }
00237       }
00238     }
00239 
00240     return tableImage;
00241   }
00242   /************************************
00243  *
00244  *
00245  *
00246  *
00247  ************************************/
00248   Mat FiducidalMarkers::rotate(const Mat  &in)
00249   {
00250     Mat out;
00251     in.copyTo(out);
00252     for (int i=0;i<in.rows;i++)
00253     {
00254       for (int j=0;j<in.cols;j++)
00255       {
00256         out.at<uchar>(i,j)=in.at<uchar>(in.cols-j-1,i);
00257       }
00258     }
00259     return out;
00260   }
00261 
00262 
00263   /************************************
00264  *
00265  *
00266  *
00267  *
00268  ************************************/
00269   int FiducidalMarkers::hammDistMarker(Mat  bits)
00270   {
00271     int ids[4][5]=
00272     {
00273       {
00274         1,0,0,0,0
00275       }
00276       ,
00277       {
00278         1,0,1,1,1
00279       }
00280       ,
00281       {
00282         0,1,0,0,1
00283       }
00284       ,
00285       {
00286         0, 1, 1, 1, 0
00287       }
00288     };
00289     int dist=0;
00290 
00291     for (int y=0;y<5;y++)
00292     {
00293       int minSum=1e5;
00294       //hamming distance to each possible word
00295       for (int p=0;p<4;p++)
00296       {
00297         int sum=0;
00298         //now, count
00299         for (int x=0;x<5;x++)
00300           sum+=  bits.at<uchar>(y,x) == ids[p][x]?0:1;
00301         if (minSum>sum) minSum=sum;
00302       }
00303       //do the and
00304       dist+=minSum;
00305     }
00306 
00307     return dist;
00308   }
00309 
00310   /************************************
00311  *
00312  *
00313  *
00314  *
00315  ************************************/
00316   int FiducidalMarkers::analyzeMarkerImage(Mat &grey,int &nRotations)
00317   {
00318 
00319     //Markers  are divided in 7x7 regions, of which the inner 5x5 belongs to marker info
00320     //the external border shoould be entirely black
00321 
00322     int swidth=grey.rows/7;
00323     for (int y=0;y<7;y++)
00324     {
00325       int inc=6;
00326       if (y==0 || y==6) inc=1;//for first and last row, check the whole border
00327       for (int x=0;x<7;x+=inc)
00328       {
00329         int Xstart=(x)*(swidth);
00330         int Ystart=(y)*(swidth);
00331         Mat square=grey(Rect(Xstart,Ystart,swidth,swidth));
00332         int nZ=countNonZero(square);
00333         if (nZ> (swidth*swidth) /2) {
00334           //            cout<<"neb"<<endl;
00335           return -1;//can not be a marker because the border element is not black!
00336         }
00337       }
00338     }
00339 
00340     //now,
00341     vector<int> markerInfo(5);
00342     Mat _bits=Mat::zeros(5,5,CV_8UC1);
00343     //get information(for each inner square, determine if it is  black or white)
00344 
00345     for (int y=0;y<5;y++)
00346     {
00347 
00348       for (int x=0;x<5;x++)
00349       {
00350         int Xstart=(x+1)*(swidth);
00351         int Ystart=(y+1)*(swidth);
00352         Mat square=grey(Rect(Xstart,Ystart,swidth,swidth));
00353         int nZ=countNonZero(square);
00354         if (nZ> (swidth*swidth) /2)  _bits.at<uchar>( y,x)=1;
00355       }
00356     }
00357     //          printMat<uchar>( _bits,"or mat");
00358 
00359     //checkl all possible rotations
00360     Mat _bitsFlip;
00361     Mat Rotations[4];
00362     Rotations[0]=_bits;
00363     int dists[4];
00364     dists[0]=hammDistMarker( Rotations[0]) ;
00365     pair<int,int> minDist( dists[0],0);
00366     for (int i=1;i<4;i++)
00367     {
00368       //rotate
00369       Rotations[i]=rotate(Rotations[i-1]);
00370       //get the hamming distance to the nearest possible word
00371       dists[i]=hammDistMarker( Rotations[i]) ;
00372       if (dists[i]<minDist.first)
00373       {
00374         minDist.first=  dists[i];
00375         minDist.second=i;
00376       }
00377     }
00378     //                  printMat<uchar>( Rotations [ minDist.second]);
00379     //                  cout<<"MinDist="<<minDist.first<<" "<<minDist.second<<endl;
00380 
00381     nRotations=minDist.second;
00382     if (minDist.first!=0)        //FUTURE WORK: correct if any error
00383       return -1;
00384     else {//Get id of the marker
00385       int MatID=0;
00386       cv::Mat bits=Rotations [ minDist.second];
00387       for (int y=0;y<5;y++)
00388       {
00389         MatID<<=1;
00390         if ( bits.at<uchar>(y,1)) MatID|=1;
00391         MatID<<=1;
00392         if ( bits.at<uchar>(y,3)) MatID|=1;
00393       }
00394       return MatID;
00395     }
00396   }
00397 
00398 
00399   /************************************
00400  *
00401  *
00402  *
00403  *
00404  ************************************/
00405   int FiducidalMarkers::detect(const Mat &in,int &nRotations)
00406   {
00407     assert(in.rows==in.cols);
00408     Mat grey;
00409     if ( in.type()==CV_8UC1) grey=in;
00410     else cv::cvtColor(in,grey,CV_BGR2GRAY);
00411     //threshold image
00412     threshold(grey, grey,125, 255, THRESH_BINARY|THRESH_OTSU);
00413 
00414     //now, analyze the interior in order to get the id
00415     //try first with the big ones
00416 
00417     return analyzeMarkerImage(grey,nRotations);;
00418     //too many false positives
00419     /*    int id=analyzeMarkerImage(grey,nRotations);
00420         if (id!=-1) return id;
00421         id=analyzeMarkerImage_type2(grey,nRotations);
00422         if (id!=-1) return id;
00423         return -1;*/
00424   }
00425 
00426   vector<int> FiducidalMarkers::getListOfValidMarkersIds_random(int nMarkers,vector<int> *excluded) throw (cv::Exception)
00427   {
00428 
00429     if (excluded!=NULL)
00430       if (nMarkers+excluded->size()>1024) throw cv::Exception(8888,"FiducidalMarkers::getListOfValidMarkersIds_random","Number of possible markers is exceeded",__FILE__,__LINE__);
00431 
00432     vector<int> listOfMarkers(1024);
00433     //set a list with all ids
00434     for (int i=0;i<1024;i++) listOfMarkers[i]=i;
00435 
00436     if (excluded!=NULL)//set excluded to -1
00437       for (size_t i=0;i<excluded->size();++i)
00438         listOfMarkers[excluded->at(i)]=-1;
00439     //random shuffle
00440     random_shuffle(listOfMarkers.begin(),listOfMarkers.end());
00441     //now, take the first  nMarkers elements with value !=-1
00442     int i=0;
00443     vector<int> retList;
00444     while (static_cast<int>(retList.size())<nMarkers) {
00445       if (listOfMarkers[i]!=-1)
00446         retList.push_back(listOfMarkers[i]);
00447       ++i;
00448     }
00449     return retList;
00450   }
00451 
00452 }
00453 


aruco
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Wed Jul 26 2017 02:17:20