aruco_selectoptimalmarkers.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  * Select the n best markers according to their distance. In other words, selects the n farthert markers in terms of hamming distance
00030  *
00031  *
00032  *
00033  *****/
00034 
00035 #include <opencv2/highgui/highgui.hpp>
00036 #include <aruco/aruco.h>
00037 #include <iostream>
00038 #include <limits>
00039 #include <aruco/arucofidmarkers.h>
00040 using namespace cv;
00041 using namespace std;
00042 
00043 int HammDist_(const cv::Mat &m1,const cv::Mat & m2)
00044 {
00045   
00046     int dist=0;
00047     for (int y=0;y<5;y++)
00048         for (int x=0;x<5;x++)
00049             if (m1.at<uchar>(y,x)!=m2.at<uchar>(y,x)) dist++;
00050     return dist;
00051 
00052 }
00053 Mat rotate(Mat  in)
00054 {
00055     Mat out;
00056     in.copyTo(out);
00057     for (int i=0;i<in.rows;i++)
00058     {
00059         for (int j=0;j<in.cols;j++)
00060         {
00061             out.at<uchar>(i,j)=in.at<uchar>(in.cols-j-1,i);
00062         }
00063     }
00064     return out;
00065 }
00066 
00067 
00068 int HammDist(const cv::Mat &m1,const cv::Mat & m2)
00069 {
00070     cv::Mat mc=m1.clone();
00071     int minD=std::numeric_limits<int>::max();
00072     for(int i=0;i<4;i++){
00073       int dist=HammDist_(mc,m2);
00074       if( dist<minD) minD=dist;
00075       mc= rotate(mc);
00076     }
00077     return minD;
00078 
00079 }
00080 
00081 int entropy(const cv::Mat &marker)
00082 {
00083   
00084   //the entropy is calcualte for each bin as the number of elements different from it in its sourroundings
00085   int totalEntropy=0;
00086     for (int y=0;y<5;y++)
00087         for (int x=0;x<5;x++){
00088             int minX=max(x-1,0);
00089             int maxX=min(x+1,5);
00090             int minY=max(y-1,0);
00091             int maxY=min(y+1,5);
00092             
00093             for(int yy=minY;yy<maxY;yy++)
00094               for(int xx=minX;xx<maxX;xx++)
00095                   if (marker.at<uchar>(y,x)!=marker.at<uchar>(yy,xx)) totalEntropy++;
00096         }
00097      
00098     return totalEntropy;
00099 }
00100 
00101 int main(int argc,char **argv)
00102 {
00103     try {
00104         if (argc<4) {
00105 
00106             //You can also use ids 2000-2007 but it is not safe since there are a lot of false positives.
00107             cerr<<"Usage: nofMarkers outbasename size [minimum_entropy(9,25)]"<<endl;
00108             return -1;
00109         }
00110         
00111       
00112         //create a vector with all markers
00113         int minimimEntropy=0;
00114         if(argc>=5) minimimEntropy=atoi(argv[4]);
00115         vector<cv::Mat> markers;
00116         vector<int> ventropy;
00117         for (int i=0;i<1024;i++){
00118             markers.push_back(aruco::FiducidalMarkers::getMarkerMat(i) );
00119             ventropy.push_back(entropy( aruco::FiducidalMarkers::getMarkerMat(i) ));
00120         }
00121           cout<<"Calculating distance matrix"<<endl;
00122         //create a matrix with all distances
00123         cv::Mat distances=cv::Mat::zeros(1024,1024,CV_32SC1);
00124         for (int i=0;i<1024;i++)
00125             for (int j=i+1;j<1024;j++)
00126                 distances.at<int>(i,j)=distances.at<int>(j,i)= HammDist (  markers[i],markers[j]);
00127         cout<<"done"<<endl;
00128             //
00129         int nMarkers=atoi(argv[1]);
00130         //select the first marker
00131         vector<bool> usedMarkers(1024,false);
00132  
00133         
00134         
00135         vector<int> selectedMarkers;
00136         //select the masker with higher entropy first
00137         int bestEntr=0;
00138         for(size_t i=0;i<ventropy.size();i++)
00139           if (ventropy[i]>ventropy[bestEntr]) bestEntr=i;
00140         selectedMarkers.push_back(bestEntr);
00141         usedMarkers[bestEntr]=true;
00142         
00143         //remove these with low entropy. Not very elegnat. Other day I will improve the algorithm
00144         //to make it multiobjective
00145         for(size_t i=0;i<ventropy.size();i++)
00146           if (ventropy[i]<minimimEntropy) usedMarkers[i]=true;
00147           
00148         cout<<"Max Entroy in ="<<bestEntr<<" "<<ventropy[bestEntr]<<endl;
00149         //add new markers according to the distance added to the global
00150         for (int i=1;i<nMarkers;i++) {
00151           int bestMarker=-1;
00152           int shorterDist=0;
00153             //select as new marker the one that maximizes mean distance to
00154             for (int j=0;j<distances.cols;j++) {
00155                 if (!usedMarkers[j]) {
00156                     int minDist=std::numeric_limits< int >::max();
00157                     for (size_t k=0;k<selectedMarkers.size();k++)
00158                         if (distances.at<int> ( selectedMarkers[k], j)<minDist) minDist=distances.at<int> ( selectedMarkers[k], j);
00159 //                  cout<<"j="<<j<<" "<<distSum<<"|"<<flush;
00160                     if (minDist>shorterDist){ 
00161                       shorterDist=minDist;
00162                       bestMarker=j;
00163                     }
00164                 }
00165             }
00166             if (bestMarker!=-1 && shorterDist>1 ){
00167               selectedMarkers.push_back(bestMarker);
00168               usedMarkers[bestMarker]=true;
00169 //            cout<<"Best id="<<bestMarker<<" dist="<<farthestDist<< endl;
00170             }
00171             else {cerr<<"COUDL NOT ADD ANY MARKER"<<endl;exit(0);}
00172 //             char c;cin>>c;
00173         }
00174         
00175         sort(selectedMarkers.begin(),selectedMarkers.end());
00176         for(size_t i=0;i<selectedMarkers.size();i++){
00177           char name[1024];
00178           sprintf(name,"%s%d.png",argv[2],selectedMarkers[i]);
00179 //        cout<<"name="<<name<<endl;
00180           cout<<selectedMarkers[i]<<" "<<flush;
00181           Mat markerImage=aruco::FiducidalMarkers::createMarkerImage(selectedMarkers[i],atoi(argv[3]));
00182           imwrite(name,markerImage);
00183         }
00184         cout<<endl;
00185         //print the minimim distance between any two  elements
00186         int minDist=std::numeric_limits<int>::max();
00187         for(size_t i=0;i<selectedMarkers.size()-1;i++)
00188           for(size_t j=i+1;j<selectedMarkers.size();j++){
00189 //          cout<<" d=" << selectedMarkers[i]<<" "<<selectedMarkers[j]<<"->"<<distances.at<int> ( selectedMarkers[i], selectedMarkers[j])<<endl;
00190             if (distances.at<int> ( selectedMarkers[i], selectedMarkers[j]) < minDist) minDist=distances.at<int> ( selectedMarkers[i], selectedMarkers[j]);
00191             
00192           }
00193             
00194         
00195    cout<<"Min Dist="<<minDist<<endl;
00196 
00197     }
00198     catch (std::exception &ex)
00199     {
00200         cout<<ex.what()<<endl;
00201     }
00202 
00203 }
00204 


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