arucofidmarkers.cpp

Go to the documentation of this file.

#include <aruco/arucofidmarkers.h>
#include <cstdio>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;
using namespace std;
namespace aruco {

/************************************
 *
 *
 *
 *
 ************************************/
Mat FiducidalMarkers::createMarkerImage(int id,int size,bool addWaterMark) throw (cv::Exception)
{
    Mat marker(size,size, CV_8UC1);
    marker.setTo(Scalar(0));
    if (0<=id && id<1024) {
        //for each line, create
        int swidth=size/7;
        int ids[4]={0x10,0x17,0x09,0x0e};
        for (int y=0;y<5;y++) {
            int index=(id>>2*(4-y)) & 0x0003;
            int val=ids[index];
            for (int x=0;x<5;x++) {
                Mat roi=marker(Rect((x+1)* swidth,(y+1)* swidth,swidth,swidth));
                if ( ( val>>(4-x) ) & 0x0001 ) roi.setTo(Scalar(255));
                else roi.setTo(Scalar(0));
            }
        }
    }
    else  throw cv::Exception(9004,"id invalid","createMarker",__FILE__,__LINE__);

    if (addWaterMark){
        char idcad[30];
        sprintf(idcad,"#%d",id);
        float ax=float(size)/100.;
        cv::putText(marker,idcad,cv::Point( 0,marker.rows - marker.rows/40),FONT_HERSHEY_TRIPLEX,ax*0.15f,cv::Scalar::all(30));
    }
    return marker;
}
cv::Mat FiducidalMarkers::getMarkerMat(int id) throw (cv::Exception)
{
    Mat marker(5,5, CV_8UC1);
    marker.setTo(Scalar(0));
    if (0<=id && id<1024) {
        //for each line, create
        int ids[4]={0x10,0x17,0x09,0x0e};
        for (int y=0;y<5;y++) {
            int index=(id>>2*(4-y)) & 0x0003;
            int val=ids[index];
            for (int x=0;x<5;x++) {
                if ( ( val>>(4-x) ) & 0x0001 ) marker.at<uchar>(y,x)=1;
                else marker.at<uchar>(y,x)=0;
            }
        }
    }
    else throw cv::Exception (9189,"Invalid marker id","aruco::fiducidal::createMarkerMat",__FILE__,__LINE__);
    return marker;
}
/************************************
 *
 *
 *
 *
 ************************************/

cv::Mat  FiducidalMarkers::createBoardImage( Size gridSize,int MarkerSize,int MarkerDistance,  BoardConfiguration& TInfo  ,vector<int> *excludedIds) throw (cv::Exception)
{



 srand(cv::getTickCount());
    int nMarkers=gridSize.height*gridSize.width;
    TInfo.resize(nMarkers);
     vector<int> ids=getListOfValidMarkersIds_random(nMarkers,excludedIds);
    for (int i=0;i<nMarkers;i++)
        TInfo[i].id=ids[i];

    int sizeY=gridSize.height*MarkerSize+(gridSize.height-1)*MarkerDistance;
    int sizeX=gridSize.width*MarkerSize+(gridSize.width-1)*MarkerDistance;
    //find the center so that the ref systeem is in it
    int centerX=sizeX/2;
    int centerY=sizeY/2;

    //indicate the data is expressed in pixels
    TInfo.mInfoType=BoardConfiguration::PIX;
    Mat tableImage(sizeY,sizeX,CV_8UC1);
    tableImage.setTo(Scalar(255));
    int idp=0;
    for (int y=0;y<gridSize.height;y++)
        for (int x=0;x<gridSize.width;x++,idp++) {
            Mat subrect(tableImage,Rect( x*(MarkerDistance+MarkerSize),y*(MarkerDistance+MarkerSize),MarkerSize,MarkerSize));
            Mat marker=createMarkerImage( TInfo[idp].id,MarkerSize);
            //set the location of the corners
            TInfo[idp].resize(4);
            TInfo[idp][0]=cv::Point3f( x*(MarkerDistance+MarkerSize),y*(MarkerDistance+MarkerSize),0);
            TInfo[idp][1]=cv::Point3f( x*(MarkerDistance+MarkerSize)+MarkerSize,y*(MarkerDistance+MarkerSize),0);
            TInfo[idp][2]=cv::Point3f( x*(MarkerDistance+MarkerSize)+MarkerSize,y*(MarkerDistance+MarkerSize)+MarkerSize,0);
            TInfo[idp][3]=cv::Point3f( x*(MarkerDistance+MarkerSize),y*(MarkerDistance+MarkerSize)+MarkerSize,0);
            for (int i=0;i<4;i++) TInfo[idp][i]-=cv::Point3f(centerX,centerY,0);
            marker.copyTo(subrect);
        }

    return tableImage;
}

/************************************
 *
 *
 *
 *
 ************************************/
cv::Mat  FiducidalMarkers::createBoardImage_ChessBoard( Size gridSize,int MarkerSize,  BoardConfiguration& TInfo ,bool centerData ,vector<int> *excludedIds) throw (cv::Exception)
{


    srand(cv::getTickCount());

    //determine the total number of markers required
    int nMarkers= 3*(gridSize.width*gridSize.height)/4;//overdetermine  the number of marker read
    vector<int> idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds);


    int sizeY=gridSize.height*MarkerSize;
    int sizeX=gridSize.width*MarkerSize;
    //find the center so that the ref systeem is in it
    int centerX=sizeX/2;
    int centerY=sizeY/2;

    Mat tableImage(sizeY,sizeX,CV_8UC1);
    tableImage.setTo(Scalar(255));
    TInfo.mInfoType=BoardConfiguration::PIX;
    int CurMarkerIdx=0;
    for (int y=0;y<gridSize.height;y++) {

        bool toWrite;
        if (y%2==0) toWrite=false;
        else toWrite=true;
        for (int x=0;x<gridSize.width;x++) {
            toWrite=!toWrite;
            if (toWrite) {
                if (CurMarkerIdx>=idsVector.size()) throw cv::Exception(999," FiducidalMarkers::createBoardImage_ChessBoard","INTERNAL ERROR. REWRITE THIS!!",__FILE__,__LINE__);
                TInfo.push_back( MarkerInfo(idsVector[CurMarkerIdx++]));

                Mat subrect(tableImage,Rect( x*MarkerSize,y*MarkerSize,MarkerSize,MarkerSize));
                Mat marker=createMarkerImage( TInfo.back().id,MarkerSize);
                //set the location of the corners
                TInfo.back().resize(4);
                TInfo.back()[0]=cv::Point3f( x*(MarkerSize),y*(MarkerSize),0);
                TInfo.back()[1]=cv::Point3f( x*(MarkerSize)+MarkerSize,y*(MarkerSize),0);
                TInfo.back()[2]=cv::Point3f( x*(MarkerSize)+MarkerSize,y*(MarkerSize)+MarkerSize,0);
                TInfo.back()[3]=cv::Point3f( x*(MarkerSize),y*(MarkerSize)+MarkerSize,0);
                if (centerData) {
                    for (int i=0;i<4;i++)
                        TInfo.back()[i]-=cv::Point3f(centerX,centerY,0);
                }
                marker.copyTo(subrect);
            }
        }
    }

    return tableImage;
}



/************************************
 *
 *
 *
 *
 ************************************/
cv::Mat  FiducidalMarkers::createBoardImage_Frame( Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,bool centerData,vector<int> *excludedIds ) throw (cv::Exception)
{

  

    srand(cv::getTickCount());
    int nMarkers=2*gridSize.height*2*gridSize.width;
    vector<int> idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds);

    int sizeY=gridSize.height*MarkerSize+MarkerDistance*(gridSize.height-1);
    int sizeX=gridSize.width*MarkerSize+MarkerDistance*(gridSize.width-1);
    //find the center so that the ref systeem is in it
    int centerX=sizeX/2;
    int centerY=sizeY/2;

    Mat tableImage(sizeY,sizeX,CV_8UC1);
    tableImage.setTo(Scalar(255));
    TInfo.mInfoType=BoardConfiguration::PIX;
    int CurMarkerIdx=0;
  int mSize=MarkerSize+MarkerDistance;
    for (int y=0;y<gridSize.height;y++) {
        for (int x=0;x<gridSize.width;x++) {
            if (y==0 || y==gridSize.height-1 || x==0 ||  x==gridSize.width-1) {
                TInfo.push_back(  MarkerInfo(idsVector[CurMarkerIdx++]));
                Mat subrect(tableImage,Rect( x*mSize,y*mSize,MarkerSize,MarkerSize));
                Mat marker=createMarkerImage( TInfo.back().id,MarkerSize);
                marker.copyTo(subrect);
                //set the location of the corners
                TInfo.back().resize(4);
                TInfo.back()[0]=cv::Point3f( x*(mSize),y*(mSize),0);
                TInfo.back()[1]=cv::Point3f( x*(mSize)+MarkerSize,y*(mSize),0);
                TInfo.back()[2]=cv::Point3f( x*(mSize)+MarkerSize,y*(mSize)+MarkerSize,0);
                TInfo.back()[3]=cv::Point3f( x*(mSize),y*(mSize)+MarkerSize,0);
                if (centerData) {
                    for (int i=0;i<4;i++)
                        TInfo.back()[i]-=cv::Point3f(centerX,centerY,0);
                }
               
            }
        }
    }

    return tableImage;
}
/************************************
 *
 *
 *
 *
 ************************************/
Mat FiducidalMarkers::rotate(const Mat  &in)
{
    Mat out;
    in.copyTo(out);
    for (int i=0;i<in.rows;i++)
    {
        for (int j=0;j<in.cols;j++)
        {
            out.at<uchar>(i,j)=in.at<uchar>(in.cols-j-1,i);
        }
    }
    return out;
}


/************************************
 *
 *
 *
 *
 ************************************/
int FiducidalMarkers::hammDistMarker(Mat  bits)
{
    int ids[4][5]=
    {
        {
            1,0,0,0,0
        }
        ,
        {
            1,0,1,1,1
        }
        ,
        {
            0,1,0,0,1
        }
        ,
        {
            0, 1, 1, 1, 0
        }
    };
    int dist=0;

    for (int y=0;y<5;y++)
    {
        int minSum=1e5;
        //hamming distance to each possible word
        for (int p=0;p<4;p++)
        {
            int sum=0;
            //now, count
            for (int x=0;x<5;x++)
                sum+=  bits.at<uchar>(y,x) == ids[p][x]?0:1;
            if (minSum>sum) minSum=sum;
        }
        //do the and
        dist+=minSum;
    }

    return dist;
}

/************************************
 *
 *
 *
 *
 ************************************/
int FiducidalMarkers::analyzeMarkerImage(Mat &grey,int &nRotations)
{

    //Markers  are divided in 7x7 regions, of which the inner 5x5 belongs to marker info
    //the external border shoould be entirely black

    int swidth=grey.rows/7;
    for (int y=0;y<7;y++)
    {
        int inc=6;
        if (y==0 || y==6) inc=1;//for first and last row, check the whole border
        for (int x=0;x<7;x+=inc)
        {
            int Xstart=(x)*(swidth);
            int Ystart=(y)*(swidth);
            Mat square=grey(Rect(Xstart,Ystart,swidth,swidth));
            int nZ=countNonZero(square);
            if (nZ> (swidth*swidth) /2) {
//              cout<<"neb"<<endl;
                return -1;//can not be a marker because the border element is not black!
            }
        }
    }

    //now,
    vector<int> markerInfo(5);
    Mat _bits=Mat::zeros(5,5,CV_8UC1);
    //get information(for each inner square, determine if it is  black or white)

    for (int y=0;y<5;y++)
    {

        for (int x=0;x<5;x++)
        {
            int Xstart=(x+1)*(swidth);
            int Ystart=(y+1)*(swidth);
            Mat square=grey(Rect(Xstart,Ystart,swidth,swidth));
            int nZ=countNonZero(square);
            if (nZ> (swidth*swidth) /2)  _bits.at<uchar>( y,x)=1;
        }
    }
//              printMat<uchar>( _bits,"or mat");

    //checkl all possible rotations
    Mat _bitsFlip;
    Mat Rotations[4];
    Rotations[0]=_bits;
    int dists[4];
    dists[0]=hammDistMarker( Rotations[0]) ;
    pair<int,int> minDist( dists[0],0);
    for (int i=1;i<4;i++)
    {
        //rotate
        Rotations[i]=rotate(Rotations[i-1]);
        //get the hamming distance to the nearest possible word
        dists[i]=hammDistMarker( Rotations[i]) ;
        if (dists[i]<minDist.first)
        {
            minDist.first=  dists[i];
            minDist.second=i;
        }
    }
//                      printMat<uchar>( Rotations [ minDist.second]);
//                      cout<<"MinDist="<<minDist.first<<" "<<minDist.second<<endl;

    nRotations=minDist.second;
    if (minDist.first!=0)        //FUTURE WORK: correct if any error
        return -1;
    else {//Get id of the marker
        int MatID=0;
        cv::Mat bits=Rotations [ minDist.second];
        for (int y=0;y<5;y++)
        {
            MatID<<=1;
            if ( bits.at<uchar>(y,1)) MatID|=1;
            MatID<<=1;
            if ( bits.at<uchar>(y,3)) MatID|=1;
        }
        return MatID;
    }
}


/************************************
 *
 *
 *
 *
 ************************************/
bool FiducidalMarkers::correctHammMarker(Mat &bits)
{
    //detect this lines with errors
    bool errors[4];
    int ids[4][5]=
    {
        {
            0,0,0,0,0
        }
        ,
        {
            0,0,1,1,1
        }
        ,
        {
            1,1,0,0,1
        }
        ,
        {
            1, 1, 1, 1, 0
        }
    };

    for (int y=0;y<5;y++)
    {
        int minSum=1e5;
        //hamming distance to each possible word
        for (int p=0;p<4;p++)
        {
            int sum=0;
            //now, count
            for (int x=0;x<5;x++)
                sum+=  bits.at<uchar>(y,x) == ids[p][x]?0:1;
            if (minSum>sum) minSum=sum;
        }
        if (minSum!=0) errors[y]=true;
        else errors[y]=false;
    }

    return true;
}

/************************************
 *
 *
 *
 *
 ************************************/
int FiducidalMarkers::detect(const Mat &in,int &nRotations)
{
    assert(in.rows==in.cols);
    Mat grey;
    if ( in.type()==CV_8UC1) grey=in;
    else cv::cvtColor(in,grey,CV_BGR2GRAY);
    //threshold image
    threshold(grey, grey,125, 255, THRESH_BINARY|THRESH_OTSU);

    //now, analyze the interior in order to get the id
    //try first with the big ones

    return analyzeMarkerImage(grey,nRotations);;
    //too many false positives
    /*    int id=analyzeMarkerImage(grey,nRotations);
        if (id!=-1) return id;
        id=analyzeMarkerImage_type2(grey,nRotations);
        if (id!=-1) return id;
        return -1;*/
}

vector<int> FiducidalMarkers::getListOfValidMarkersIds_random(int nMarkers,vector<int> *excluded) throw (cv::Exception)
{

    if (excluded!=NULL)
        if (nMarkers+excluded->size()>1024) throw cv::Exception(8888,"FiducidalMarkers::getListOfValidMarkersIds_random","Number of possible markers is exceeded",__FILE__,__LINE__);

    vector<int> listOfMarkers(1024);
//set a list with all ids
    for (int i=0;i<1024;i++) listOfMarkers[i]=i;

    if (excluded!=NULL)//set excluded to -1
        for (size_t i=0;i<excluded->size();i++)
            listOfMarkers[excluded->at(i)]=-1;
//random shuffle
    random_shuffle(listOfMarkers.begin(),listOfMarkers.end());
//now, take the first  nMarkers elements with value !=-1
    int i=0;
    vector<int> retList;
    while (retList.size()<nMarkers) {
        if (listOfMarkers[i]!=-1)
            retList.push_back(listOfMarkers[i]);
        i++;
    }
    return retList;
}

}