Библиотека cvQCodeBeacons

Назначение библиотеки

Поддержка визуальных маяков типа "QCode".

Функции (описание)

mycvGetQCodeBeacons

Определить на переданном изображении визуальные маяки типа "QCode" и их локальные координаты относительно камеры;

Файлы библиотеки

CvQCodeBeacons.h

<source lang="cpp">

1. ifndef cvQCodeBeaconsH
2. define cvQCodeBeaconsH

//---------------------------------------------------------------------------

1. include "cv.h" // includes OpenCV definitions
2. include "highgui.h" // includes highGUI definitions

struct CvQCodeBeacon {

 //Номер маяка (0..2^15-1)
 long int id;

 // Поля для заполнения внешней программой:
 long double rx,ry,rz; //Локальные координаты маяка
 long double x,y,z; //Глобальные координаты маяка
 long double d; //Расстояние от робота до маяка

 //Поля заполняемые функцией нахождения маяков на изображении
 long double bx,by; //Один из углов маяка
 long double x1,y1,x2,y2; //Опорные вектора параллелепипеда маяка

 //Ссылка на блок информации о следующем маяке
 CvQCodeBeacon* next; //NULL, если это последний маяк
};

CvQCodeBeacon* mycvGetQCodeBeacons(IplImage *img, int ThresholdSteps);

1. endif

</source>

CvQCodeBeacons.cpp

<source lang="cpp">

1. pragma hdrstop
2. include "CvQCodeBeacons.h"

//---------------------------------------------------------------------------

1. pragma package(smart_init)

double sqr(double a){
  return a*a;
};

CvQCodeBeacon* mycvGetQCodeBeacons(IplImage *img, int ThresholdSteps){

 CvQCodeBeacon* fbeacon = NULL;

 CvSize szSrc; szSrc.width=img->width; szSrc.height=img->height;
 CvSize szDiv2; szDiv2.width=szSrc.width/2; szDiv2.height=szSrc.height/2;

 IplImage* pyr = cvCreateImage(szDiv2, IPL_DEPTH_8U, 3);
 IplImage* tmp = cvCreateImage(szSrc, IPL_DEPTH_8U, 3);

 cvPyrDown( img, pyr, 7 );
 cvPyrUp(   pyr, tmp, 7 );

 IplImage*  gray = cvCreateImage( szSrc, 8, 1 );
 IplImage* tgray = cvCreateImage( szSrc, 8, 1 );

 cvSetImageCOI( tmp, 2 );
 cvCopy( tmp, tgray, 0 );

 CvMemStorage* storage=cvCreateMemStorage(0);
 CvSeq* contours, result;

 CvPoint rect20[20];
 CvPoint *rect=rect20;

 for(int l=0; l<ThresholdSteps; l++){

   //Отфильтруем изображение
   if(l==0){
     cvCanny( tgray, gray, 0, 100, 5 );
     cvDilate( gray, gray, 0, 1 );
   }else{
     cvThreshold( tgray, gray, (l+1)*255/ThresholdSteps, 255, CV_THRESH_BINARY );
   };

   //Найдем все контуры на изображении
   cvFindContours( gray, storage, &contours, sizeof(CvContour),
                   CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );

   //Для каждого из найденных контуров
   while( contours )
   {
     //Приблизим его многоугольником
     CvSeq* result = cvApproxPoly( contours, sizeof(CvContour), storage,
     CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 );

     //Извлечем количество вершин многоугольника
     int count = result->total;

     if(
       count == 4 //Это должен быть четырехугольник
       && fabs(cvContourArea(result,CV_WHOLE_SEQ)) > 100 //Достаточной площади
       && cvCheckContourConvexity(result) //Выпуклый
     ){

       //Флаг, который будем сбрасывать, если что-то не так при проверке
       int isPara=1;

       //Выпишем координаты его вершин
       double x[5],y[5];
       for(int i=0; i < count; i++){
         CvPoint *pt=(CvPoint*)cvGetSeqElem( result, i );
         x[i]=pt->x;
         y[i]=szSrc.height-pt->y;
       }; x[count]=x[0]; y[count]=y[0];

       //Посчитаем длины сторон полученного четырехугольника
       double l[5];
       for(int i=1; i<=4; i++){
         l[i]=sqrt(sqr(x[i-1]-x[i])+sqr(y[i-1]-y[i]));
       };

       //Проверим все стороны, если они сильно отличаются по длине, то отбой
       if( fabs(l[1]/(l[1]+l[3])-0.5)>0.1 ){ isPara=0; };
       if( fabs(l[2]/(l[2]+l[4])-0.5)>0.1 ){ isPara=0; };
       if( fabs(l[2]/(l[2]+l[3])-0.5)>0.2 ){ isPara=0; };
       if( l[1]+l[2]>800 ){ isPara=0; };

       //Проверим один из углов - если он сильно отличается от прямого, то отбой
       double s_mul=(x[1]-x[0])*(x[2]-x[1])+(y[1]-y[0])*(y[2]-y[1]);
       double cos_a=s_mul/(l[1]*l[2]);
       if( cos_a > 0.3 ){ isPara=0; };

       //Проверим, что в этих координатах еще не находили какой-либо маяк
       if( isPara ){
         double cx=(x[0]+x[1]+x[2]+x[3])/4;
         double cy=(y[0]+y[1]+y[2]+y[3])/4;
         CvQCodeBeacon *bc=fbeacon;
         while( bc ){
           double dist=sqrt(sqr(cx-bc->x)+sqr(cy-bc->y));
           if( dist<5 ){ isPara=0; }; //Если что-то очень рядом было, то отбой
           bc=bc->next;
         };
       };

       //Если прошли все первичные проверки
       if( isPara ){

         double res[7][7];
         int K=7;

         int area=1; // Будем пробовать искать координаты вершин маяка в рамках -area..+area

         for(long int dx1=-area; dx1<=area; dx1++){
         for(long int dy1=-area; dy1<=area; dy1++){
         for(long int dx2=-area; dx2<=area; dx2++){
         for(long int dy2=-area; dy2<=area; dy2++){
         for(long int dx3=-area; dx3<=area; dx3++){
         for(long int dy3=-area; dy3<=area; dy3++){

           double xC[4],yC[4];
           xC[1]=x[1]+dx1; yC[1]=y[1]+dy1;
           xC[2]=x[2]+dx2; yC[2]=y[2]+dy2;
           xC[3]=x[3]+dx3; yC[3]=y[3]+dy3;

           double base_x=xC[2], base_y=yC[2];
           double v1x=xC[1]-xC[2], v1y=yC[1]-yC[2];
           double v2x=xC[3]-xC[2], v2y=yC[3]-yC[2];

           uchar* datas=tmp->imageDataOrigin;
           long int steps=tmp->widthStep;
           double vmax=0,vmin=255;

           //Заполним матрицу клеток маяка
           for(long int d1=0; d1<K; d1++){
           for(long int d2=0; d2<K; d2++){
             long int d_x=base_x+(0.5+d1)*v1x/K+(0.5+d2)*v2x/K;
             long int d_y=base_y+(0.5+d1)*v1y/K+(0.5+d2)*v2y/K;
             if( d_x<0 || d_x>szSrc.width || d_y<0 || d_y>szSrc.height ){
               res[d1][d2]=1000;
             }else{
               long int adr=d_x*3+(szSrc.height-d_y)*steps+2;
               res[d1][d2]=datas[adr];
             };
             if( res[d1][d2]>vmax ){ vmax=res[d1][d2]; };
             if( res[d1][d2]<vmin ){ vmin=res[d1][d2]; };
           };};

           for(int d1=0; d1<K; d1++){
             for(int d2=0; d2<K; d2++){
               res[d1][d2]= res[d1][d2]<(vmax+vmin)/2 ? 1 : 0;
             };
           };

           int isGood=1;

           //Проверим на то, что по периметру центрального квадрата маяка размером 7х7 идут черные клетки, иначе ошибка
           for(int i=0; i<K; i++){
             if( res[0][i]==0 ){ isGood=0; i=K; };
             if( res[K-1][i]==0 ){ isGood=0; i=K; };
             if( res[i][0]==0 ){ isGood=0; i=K; };
             if( res[i][K-1]==0 ){ isGood=0; i=K; };
           };

           //Проверим, что во внутреннем квадрате 5х5 маяка в углах всего 1 клетка закрашена (маркер поворота)
           if( res[1][1]+res[5][1]+res[5][5]+res[1][5] != 1 ){ isGood=0; };

           //Если последние две проверки прошли, то движемся дальше.
           if( isGood==1 ){

             //Развернем нужным нам образом матрицу маяка
             double tmp[7][7];
             for(int d1=0; d1<K; d1++){
             for(int d2=0; d2<K; d2++){
               int dd1=res[1][1]+res[1][5]==1 ? d1 : K-1-d1;
               int dd2=res[1][1]+res[5][1]==1 ? d2 : K-1-d2;
               tmp[d1][d2]=res[dd1][dd2];
             };};

             for(int d1=0; d1<K; d1++){
             for(int d2=0; d2<K; d2++){
               int dd1=tmp[1][2]==1 ? d1 : d2;
               int dd2=tmp[1][2]==1 ? d2 : d1;
               res[d1][d2]=tmp[dd1][dd2];
             };};

             int code[20]; for(int i=0; i<20; i++){ code[i]=0; };

             long int beacon=0;

             //Рассчитаем номер маяка и проверим его контрольную сумму
             if( isGood==1 ){
               int checkbits=0;
               for(int i=2; i<=4; i++){ code[i-1]=res[5][i]; };
               for(int i=1; i<=5; i++){ code[i+3]=res[4][i]; code[i+8]=res[3][i]; };
               for(int i=2; i<=5; i++){ code[i+12]=res[2][i]; };
               for(int i=3; i<=4; i++){ code[i+15]=res[1][i]; };
               for(int j=0; j<5; j++){
                 int bits=0;
                 for(int i=0; i<4; i++){ bits = (bits << 1) + code[j*4+i]; };
                 checkbits = checkbits ^ bits;
                 if( j<4 ){ beacon = (beacon << 4) + bits; };
               };
               if( checkbits != 13 ){ isGood=0; };
             };

             //Если контрольная сумма прошла, тогда зарегистрируем новый обнаруженный маяк
             if( isGood==1 ){
               CvQCodeBeacon* cur=fbeacon;
               while( cur && cur->next && cur->id != beacon ){ cur=cur->next; };
               if( !cur ){
                 cur=new CvQCodeBeacon();
                 fbeacon=cur;
                 cur->next=NULL;
               }else if( cur->id != beacon ){
                 cur->next=new CvQCodeBeacon();
                 cur=cur->next;
                 cur->next=NULL;
               };
               cur->id=beacon;
               cur->x=base_x+(v1x+v2x)/2;
               cur->y=base_y+(v1y+v2y)/2;
               cur->bx=base_x;
               cur->by=base_y;
               cur->x1=v1x;
               cur->y1=v1y;
               cur->x2=v2x;
               cur->y2=v2y;
               long double size=sqrt(sqr(v1x+v2x)+sqr(v1y+v2y));

               //Рассчитаем коордитаны маяка относительно камеры (тут надо будет поработать еще, пока тупо прошито под 1 камеру)
               cur->d=(floor(12500/size))/100;
               cur->ry=((cur->x) - szDiv2.width)*(cur->d)/900;
               cur->rz=((cur->y) - szDiv2.height)*(cur->d)/900;

               long double sqd=sqr(cur->d)-sqr(cur->ry)-sqr(cur->rz);

               if(sqd<0.04){
                 sqd=0.04;
               };
               cur->rx=sqrt(sqd);

               dx1=9; dy1=9; dx2=9; dy2=9; dx3=9; dy3=9;
             };
           };

         };};};};};};
       };
     };
     contours = contours->h_next;
   };
 };

 cvReleaseMemStorage( &storage );

 cvReleaseImage(&pyr);
 cvReleaseImage(&tmp);
 cvReleaseImage(&gray);
 cvReleaseImage(&tgray);

 return fbeacon;
};

</source>