highlyreliablemarkers.cpp
Go to the documentation of this file.
00001 /*****************************
00002 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification, are
00005 permitted provided that the following conditions are met:
00006 
00007    1. Redistributions of source code must retain the above copyright notice, this list of
00008       conditions and the following disclaimer.
00009 
00010    2. Redistributions in binary form must reproduce the above copyright notice, this list
00011       of conditions and the following disclaimer in the documentation and/or other materials
00012       provided with the distribution.
00013 
00014 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00015 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00019 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00020 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00021 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00022 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00023 
00024 The views and conclusions contained in the software and documentation are those of the
00025 authors and should not be interpreted as representing official policies, either expressed
00026 or implied, of Rafael Muñoz Salinas.
00027 ********************************/
00028 
00029 #include <aruco/highlyreliablemarkers.h>
00030 
00031 namespace aruco {
00032 
00033   // static variables from HighlyReliableMarkers. Need to be here to avoid linking errors
00034   Dictionary HighlyReliableMarkers::_D;
00035   HighlyReliableMarkers::BalancedBinaryTree HighlyReliableMarkers::_binaryTree;
00036   unsigned int HighlyReliableMarkers::_n, HighlyReliableMarkers::_ncellsBorder, HighlyReliableMarkers::_correctionDistance;
00037   int HighlyReliableMarkers::_swidth;
00038 
00039 
00042   MarkerCode::MarkerCode(unsigned int n) {
00043     // resize bits vectors and initialize to 0
00044     for(unsigned int i=0; i<4; i++) {
00045       _bits[i].resize(n*n); 
00046       for(unsigned int j=0; j<_bits[i].size(); j++) _bits[i][j]=0;
00047       _ids[i] = 0; // ids are also 0
00048     }
00049     _n = n;
00050   };
00051   
00052   
00055   MarkerCode::MarkerCode(const MarkerCode& MC)
00056   {
00057     for(unsigned int i=0; i<4; i++) {
00058       _bits[i] = MC._bits[i];
00059       _ids[i] = MC._ids[i];
00060     }
00061     _n = MC._n;
00062   }
00063 
00064   
00065   
00068   void MarkerCode::set(unsigned int pos, bool val) {
00069     // if not the same value
00070     if( get(pos) != val ) {
00071       for(unsigned int i=0; i<4; i++) { // calculate bit coordinates for each rotation
00072     unsigned int y=pos/n(), x=pos%n(); // if rotation 0, dont do anything
00073         // else calculate bit position in that rotation
00074     if(i==1) { unsigned int aux=y; y=x; x=n()-aux-1; }
00075         else if(i==2) { y=n()-y-1; x=n()-x-1; }
00076     else if(i==3) { unsigned int aux=y; y=n()-x-1; x=aux; }
00077     unsigned int rotPos = y*n()+x; // calculate position in the unidimensional string
00078         _bits[i][rotPos] = val; // modify value
00079         // update identifier in that rotation
00080     if(val==true) _ids[i] += (unsigned int)pow(float(2),float(rotPos)); // if 1, add 2^pos
00081     else _ids[i] -= (unsigned int)pow(float(2),float(rotPos)); // if 0, substract 2^pos
00082       }   
00083     }
00084   }
00085   
00086   
00089   unsigned int MarkerCode::selfDistance(unsigned int &minRot) {
00090     unsigned int res = _bits[0].size(); // init to n*n (max value)
00091     for(unsigned int i=1; i<4; i++) { // self distance is not calculated for rotation 0
00092       unsigned int hammdist = hammingDistance(_bits[0], _bits[i]);
00093       if(hammdist<res) {
00094         minRot = i;
00095         res = hammdist;
00096       }
00097     }
00098     return res;
00099   }
00100   
00101   
00104   unsigned int MarkerCode::distance(MarkerCode m, unsigned int &minRot) {
00105     unsigned int res = _bits[0].size(); // init to n*n (max value)
00106     for(unsigned int i=0; i<4; i++) {
00107       unsigned int hammdist = hammingDistance(_bits[0], m.getRotation(i));
00108       if(hammdist<res) {
00109         minRot = i;
00110         res = hammdist;
00111       }      
00112     }
00113     return res;    
00114   };
00115   
00116 
00119   void MarkerCode::fromString(std::string s)
00120   {
00121     for(unsigned int i=0; i<s.length(); i++) {
00122       if(s[i]=='0') set(i, false);
00123       else set(i, true);
00124     }
00125   }
00126 
00129   std::string MarkerCode::toString()
00130   {
00131     std::string s;
00132     s.resize(size());
00133     for(unsigned int i=0; i<size(); i++) {
00134       if(get(i)) s[i]='1';
00135       else s[i]='0';
00136     }
00137     return s;
00138   }  
00139   
00140   
00143   cv::Mat MarkerCode::getImg(unsigned int pixSize) {
00144     const unsigned int borderSize=1;
00145     unsigned int nrows = n()+2*borderSize;
00146     if(pixSize%nrows != 0) pixSize = pixSize + nrows - pixSize%nrows;
00147     unsigned int cellSize = pixSize / nrows;
00148     cv::Mat img(pixSize, pixSize, CV_8U, cv::Scalar::all(0)); // create black image (init image to 0s)
00149     // double for to go over all the cells
00150     for(unsigned int i=0; i<n(); i++) {
00151       for(unsigned int j=0; j<n(); j++) {
00152         if(_bits[0][i*n()+j]!=0) { // just draw if it is 1, since the image has been init to 0
00153           // double for to go over all the pixels in the cell
00154       for(unsigned int k=0; k<cellSize; k++) {
00155         for(unsigned int l=0; l<cellSize; l++) {
00156               img.at<uchar>((i+borderSize)*cellSize+k, (j+borderSize)*cellSize+l) = 255;
00157             }
00158           }
00159         }
00160       }
00161     }
00162     return img;    
00163   }  
00164   
00165   
00168   unsigned int MarkerCode::hammingDistance(std::vector<bool> m1, std::vector<bool> m2) {
00169     unsigned int res=0;
00170     for(unsigned int i=0; i<m1.size(); i++)
00171       if(m1[i]!=m2[i]) res++;
00172     return res;
00173   };  
00174   
00175   
00176   
00177   
00178   
00179   
00182   bool Dictionary::fromFile(std::string filename) {
00183     cv::FileStorage fs(filename, cv::FileStorage::READ);
00184     int nmarkers, markersize;
00185     
00186     // read number of markers
00187     fs["nmarkers"] >> nmarkers; // cardinal of D
00188     fs["markersize"] >> markersize; // n
00189        
00190     // read each marker info
00191     for (int i=0; i<nmarkers; i++) {  
00192         std::string s;
00193         fs["marker_" + toStr(i)] >> s;
00194         MarkerCode m(markersize);
00195         m.fromString(s);
00196         push_back(m);
00197     }
00198     fs.release();   
00199     return true;
00200   };
00201   
00204   bool Dictionary::toFile(std::string filename) {
00205     cv::FileStorage fs(filename, cv::FileStorage::WRITE);
00206     // save number of markers
00207     fs << "nmarkers" << (int)size(); // cardinal of D 
00208     fs << "markersize" << (int)( (*this)[0].n() ); // n
00209     // save each marker code
00210     for (unsigned int i=0; i<size(); i++) {
00211         std::string s =  ((*this)[i]).toString();
00212         fs << "marker_" + toStr(i) << s;
00213     }
00214     fs.release();  
00215     return true;
00216   };
00217   
00220   unsigned int Dictionary::distance(MarkerCode m, unsigned int &minMarker, unsigned int &minRot) {
00221     unsigned int res = m.size();
00222     for(unsigned int i=0; i<size(); i++) {
00223       unsigned int minRotAux;
00224       unsigned int distance = (*this)[i].distance(m, minRotAux);
00225       if(distance<res) {
00226         minMarker = i;
00227         minRot = minRotAux;
00228         res=distance;
00229       }
00230     }
00231     return res;
00232   }
00233   
00234   
00237   unsigned int Dictionary::minimunDistance()
00238   {
00239     if(size()==0) return 0;
00240     unsigned int minDist = (*this)[0].size();
00241     // for each marker in D
00242     for(unsigned int i=0; i<size(); i++) {
00243       // calculate self distance of the marker
00244       minDist = std::min( minDist, (*this)[i].selfDistance() );
00245       
00246       // calculate distance to all the following markers
00247       for(unsigned int j=i+1; j<size(); j++) {
00248         minDist = std::min( minDist, (*this)[i].distance((*this)[j]) );
00249       }
00250     }
00251     return minDist;
00252   }
00253   
00254   
00255   
00256   
00257   
00258   
00259   
00260   
00261   
00262   
00265   bool HighlyReliableMarkers::loadDictionary(Dictionary D){  
00266     if(D.size()==0) return false;
00267     _D = D;
00268     _n = _D[0].n();
00269     _ncellsBorder = (_D[0].n()+2);
00270     _correctionDistance = (unsigned int)floor( (_D.minimunDistance()-1)/2. ); //maximun correction distance
00271     
00272     _binaryTree.loadDictionary(&D);   
00273     
00274     return true;
00275     
00276   };
00277   
00278   bool HighlyReliableMarkers::loadDictionary(std::string filename){ 
00279     Dictionary D;
00280     D.fromFile(filename); 
00281     return loadDictionary(D);
00282   };  
00283     
00284   
00287   int HighlyReliableMarkers::detect(const cv::Mat& in, int& nRotations)
00288   {  
00289 
00290     assert(in.rows==in.cols);
00291     cv::Mat grey;
00292     if ( in.type()==CV_8UC1) grey=in;
00293     else cv::cvtColor(in,grey,CV_BGR2GRAY);
00294     //threshold image
00295     cv::threshold(grey, grey,125, 255, cv::THRESH_BINARY|cv::THRESH_OTSU);
00296     _swidth=grey.rows/_ncellsBorder;    
00297     
00298     // check borders, even not necesary for the highly reliable markers
00299     //if(!checkBorders(grey)) return -1; 
00300     
00301     // obtain inner code
00302     MarkerCode candidate = getMarkerCode(grey);
00303 
00304     // search each marker id in the balanced binary tree
00305     unsigned int orgPos;
00306     for(unsigned int i=0; i<4; i++) {
00307       if(_binaryTree.findId( candidate.getId(i), orgPos )) {
00308           nRotations = i;
00309           return candidate.getId(i);
00310           //return orgPos;
00311       }
00312     }
00313     
00314     // alternative version without using the balanced binary tree (less eficient)
00315     //     for(uint i=0; i<_D.size(); i++) {
00316     //       for(uint j=0; j<4; j++) {
00317     //  if(_D[i].getId() == candidate.getId(j)) {
00318     //    nRotations = j;
00319     //    return candidate.getId(j);
00320     //  }
00321     //       }
00322     //     }
00323     
00324     // correct errors
00325     unsigned int minMarker, minRot;
00326     if(_D.distance(candidate, minMarker, minRot) <= _correctionDistance) {
00327       nRotations = minRot;
00328       //return minMarker;
00329      return _D[minMarker].getId();
00330     }
00331         
00332     return -1;
00333 
00334   }  
00335   
00336   
00339   bool HighlyReliableMarkers::checkBorders(cv::Mat grey) {
00340     for (int y=0;y<_ncellsBorder;y++)
00341     {
00342         int inc=_ncellsBorder-1;
00343         if (y==0 || y==_ncellsBorder-1) inc=1;//for first and last row, check the whole border
00344         for (int x=0;x<_ncellsBorder;x+=inc)
00345         {
00346             int Xstart=(x)*(_swidth);
00347             int Ystart=(y)*(_swidth);
00348             cv::Mat square=grey(cv::Rect(Xstart,Ystart,_swidth,_swidth));
00349             int nZ=cv::countNonZero(square);
00350             if (nZ> (_swidth*_swidth) /2) {
00351                 return false;//can not be a marker because the border element is not black!
00352             }
00353         }
00354     }    
00355     return true;
00356   }
00357   
00360   MarkerCode HighlyReliableMarkers::getMarkerCode(cv::Mat grey) {
00361     MarkerCode candidate( _n );
00362     for (int y=0;y<_n;y++)
00363     {
00364         for (int x=0;x<_n;x++)
00365         {
00366             int Xstart=(x+1)*(_swidth);
00367             int Ystart=(y+1)*(_swidth);
00368             cv::Mat square=grey(cv::Rect(Xstart,Ystart,_swidth,_swidth));
00369             int nZ=countNonZero(square);
00370             if (nZ> (_swidth*_swidth) /2)  candidate.set(y*_n+x, 1);
00371         }
00372      } 
00373      return candidate;
00374    };
00375    
00376    
00377   
00380   void HighlyReliableMarkers::BalancedBinaryTree::loadDictionary(Dictionary *D) {
00381     // create _orderD wich is a sorted version of D
00382     _orderD.clear();
00383     for(unsigned int i=0; i<D->size(); i++) {
00384       _orderD.push_back( std::pair<unsigned int,unsigned int>( (*D)[i].getId() ,i) );
00385     }
00386     std::sort(_orderD.begin(), _orderD.end());
00387     
00388     // calculate the number of levels of the tree 
00389     unsigned int levels=0;
00390     while( pow(float(2),float(levels)) <= _orderD.size() ) levels++;
00391     //       levels-=1; // only count full levels
00392     
00393     // auxiliar vector to know which elements are already in the tree
00394     std::vector<bool> visited;
00395     visited.resize(_orderD.size());
00396     for(unsigned int i=0; i<_orderD.size(); i++) visited[i]=false;
00397     
00398     // calculate position of the root element
00399     unsigned int rootIdx = _orderD.size()/2;
00400     visited[rootIdx] = true; // mark it as visited
00401     
00402     // auxiliar vector to store the ids intervals (max and min) during the creation of the tree
00403     std::vector< std::pair<unsigned int, unsigned int> > intervals;
00404     // first, add the two intervals at each side of root element
00405     intervals.push_back( std::pair<unsigned int, unsigned int>(0,rootIdx) );
00406     intervals.push_back( std::pair<unsigned int, unsigned int>(rootIdx,_orderD.size()) );
00407 
00408     // init the tree
00409     _binaryTree.clear();
00410     _binaryTree.resize(_orderD.size());
00411     
00412     // add root information to the tree (make sure child do not coincide with self root for small sizes of D)
00413     if(!visited[(0+rootIdx)/2]) _binaryTree[rootIdx].first = (0+rootIdx)/2;
00414     else _binaryTree[rootIdx].first = -1;
00415     if(!visited[(rootIdx+_orderD.size())/2]) _binaryTree[rootIdx].second = (rootIdx+_orderD.size())/2;
00416     else _binaryTree[rootIdx].second = -1;
00417             
00418     // for each tree level
00419     for(unsigned int i=1; i<levels; i++) {
00420       unsigned int nintervals = intervals.size(); // count number of intervals and process them
00421       for(unsigned int j=0; j<nintervals; j++) {
00422         // store interval information and delete it
00423     unsigned int lowerBound, higherBound;
00424         lowerBound = intervals.back().first;
00425         higherBound = intervals.back().second;
00426         intervals.pop_back();
00427         
00428         // center of the interval
00429     unsigned int center = (higherBound + lowerBound)/2;
00430         
00431         // if center not visited, continue
00432         if(!visited[center])    visited[center]=true;
00433         else continue;
00434         
00435         // calculate centers of the child intervals
00436     unsigned int lowerChild = (lowerBound + center)/2;
00437     unsigned int higherChild = (center + higherBound)/2;
00438         
00439         // if not visited (lower child)
00440         if(!visited[lowerChild]) {
00441       intervals.insert( intervals.begin(), std::pair<unsigned int, unsigned int>(lowerBound, center) ); // add the interval to analyze later
00442           _binaryTree[center].first = lowerChild; // add as a child in the tree
00443         }
00444         else _binaryTree[center].first = -1; // if not, mark as no child
00445 
00446         // (higher child, same as lower child)
00447         if(!visited[higherChild]) {
00448       intervals.insert( intervals.begin(), std::pair<unsigned int, unsigned int>(center, higherBound) );
00449           _binaryTree[center].second = higherChild;
00450         }
00451         else _binaryTree[center].second = -1;
00452         
00453       }
00454     }      
00455     
00456     // print tree
00457     // for(uint i=0; i<_binaryTree.size(); i++) std::cout << _binaryTree[i].first << " " << _binaryTree[i].second << std::endl;
00458     // std::cout << std::endl;
00459     
00460   };   
00461   
00462   
00465   bool HighlyReliableMarkers::BalancedBinaryTree::findId(unsigned int id, unsigned int &orgPos) {
00466     unsigned int pos = _root; // first position is root
00467     while(pos!=-1) { // while having a valid position
00468       unsigned int posId = _orderD[pos].first; // calculate id of the node
00469       if(posId == id ) {
00470         orgPos = _orderD[pos].second;
00471         return true; // if is the desire id, return true
00472       }
00473       else if(posId < id) pos = _binaryTree[pos].second; // if desired id is higher, look in higher child
00474       else pos = _binaryTree[pos].first; // if it is lower, look in lower child
00475     }
00476     return false; // if nothing found, return false
00477   }
00478   
00479   
00480 };


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Thu Aug 27 2015 12:23:55