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


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sun Oct 5 2014 22:12:49