src/camera/color-finder/control/CRecognition.cpp

   1 #include <CRecognition.h>
   2
   3 #define min(a,b) ((a) < (b) ? (a) : (b))
   4 #define max(a,b) ((a) > (b) ? (a) : (b))
   5
   6 //Vymazani promennych a jejich nastaveni na pocatecni hodnoty
   7 CRecognition::CRecognition()
   8 {
   9         memset(learned,0,sizeof(unsigned char)*3);
  10         memset(colorArray,0,64*64*64);
  11         learned[0] = 10;
  12         learned[1] = 10;
  13         learned[2] = 60;
  14         tolerance = 30;
  15         debug = false;
  16 }
  17
  18 CRecognition::~CRecognition()
  19 {
  20 }
  21
  22 //zvysi prah t, toleranci podobnosti vzorovemu pixelu
  23 void CRecognition::increaseTolerance()
  24 {
  25         tolerance+=5;
  26         if (tolerance > 400) tolerance = 400;
  27         fprintf(stdout,"Tolerance: %i\n",tolerance);
  28 }
  29
  30 //snizi prah t, toleranci podobnosti vzorovemu pixelu
  31 void CRecognition::decreaseTolerance()
  32 {
  33         tolerance-=5;
  34         if (tolerance < 0) tolerance = 0;
  35         fprintf(stdout,"Tolerance: %i\n",tolerance);
  36 }
  37
  38 //smaze indexovaci tabulku
  39 void CRecognition::resetColorMap()
  40 {
  41         memset(colorArray,0,64*64*64);
  42 }
  43
  44 //nauci se dany pixel
  45 void CRecognition::learnPixel(unsigned char* a,unsigned char color [3])
  46 {
  47         if (a) {
  48         //ulozi pixel do vzoroveho
  49         for (int i =0;i<3;i++) learned[i] = a[i];
  50         }else {
  51         for (int i =0;i<3;i++) learned[i] = color[i];
  52         }
  53         //prevede nauceny pixel do HSV
  54         rgbToHsv(learned[0],learned[1],learned[2],&learnedHue,&learnedSaturation,&learnedValue);
  55
  56         //z daneho vzoru vytvori indexovaci tabulku
  57         unsigned char u[3];
  58         for (int r=0;r<256;r+=4){
  59                 u[0] = r;
  60                 for (int g=0;g<256;g+=4){
  61                         u[1] = g;
  62                         for (int b=0;b<256;b+=4){
  63                                 u[2] = b;
  64                                 int i = ((r/4)*64+g/4)*64+b/4;
  65                                 colorArray[i] = evaluatePixel3(u);
  66                         }
  67                 }
  68         }
  69         fprintf(stdout,"Learned RGB: %i %i %i, HSV: %i %i %i\n",learned[0],learned[1],learned[2],learnedHue,learnedSaturation,learnedValue);
  70 }
  71
  72 //vrati podobnost pixelu metodou indexovaci tabulky - metoda z dilu IV
  73 int CRecognition::evaluatePixelFast(unsigned char *a)
  74 {
  75         int b = ((a[0]/4)*64+a[1]/4)*64+a[2]/4;
  76         return colorArray[b];
  77 }
  78
  79 //podobnost pixelu - prvni metoda z dilu III
  80 float CRecognition::evaluatePixel1(unsigned char* a)
  81 {
  82         float result = 1;
  83         for (int i =0;i<3;i++){
  84                 result += pow((int)a[i]-(int)learned[i],2);
  85         }
  86         return 1/result;
  87 }
  88
  89 //podobnost pixelu - druha metoda z dilu III
  90 float CRecognition::evaluatePixel2(unsigned char* a)
  91 {
  92         float result = 0;
  93         for (int i =0;i<3;i++){
  94                 result += pow((int)a[i]-(int)learned[i],2);
  95         }
  96         result = sqrt(result);
  97         if (result > tolerance) result = 0; else result = 1;
  98         return result;
  99 }
 100
 101 //podobnost pixelu - druha metoda z dilu III
 102 float CRecognition::evaluatePixel3(unsigned char* a)
 103 {
 104         float result = 0;
 105         unsigned int h;
 106         unsigned char s,v;
 107         rgbToHsv(a[0],a[1],a[2],&h,&s,&v);
 108         if (v > 40 && s > 40){
 109                 result = result + pow((int)h-(int)learnedHue,2);
 110                 result = result + pow((int)s-(int)learnedSaturation,2)/4;
 111                 result = result + pow((int)v-(int)learnedValue,2)/16;
 112         }else{
 113                 return 0;
 114         }
 115         result = sqrt(result);
 116         if (result > tolerance) result = 0; else result = 1;
 117         return result;
 118 }
 119
 120 //segmentace obrazu - metoda z dilu IV
 121 SPixelPosition CRecognition::findSegment(CRawImage* image)
 122 {
 123         SPixelPosition result;
 124         result.x = 320;
 125         result.y = 240;
 126         result.info = false;
 127
 128         int expand[4] = {image->width,-image->width,1,-1};
 129         int stack[image->width*image->height];
 130         int stackPosition = 0;
 131
 132         int numSegments = 0;
 133         int buffer[image->width*image->height];
 134         int len = image->width*image->height;
 135
 136         //oznacime oblasti s hledanou barvou
 137         for (int i = 0;i<len;i++) buffer[i] = -evaluatePixelFast(&image->data[3*i]);
 138
 139         //'ukrojime' okraje obrazu
 140         int pos =  (image->height-1)*image->width;
 141         for (int i = 0;i<image->width;i++){
 142                          buffer[i] = 0;
 143                          buffer[pos+i] = 0;
 144         }
 145         for (int i = 0;i<image->height;i++){
 146                          buffer[image->width*i] = 0;
 147                          buffer[image->width*i+image->width-1] = 0;
 148         }
 149
 150         //zacneme prohledavani
 151         int position = 0;
 152         for (int i = 0;i<len;i++){
 153                 //pokud je nalezen pixel s hledanou barvou,
 154                 if (buffer[i] < 0 && numSegments < MAX_SEGMENTS){
 155
 156                         //zalozime dalsi segment
 157                         buffer[i] = ++numSegments;
 158                         segmentArray[numSegments-1].size = 1;
 159                         segmentArray[numSegments-1].x = i%image->width;
 160                         segmentArray[numSegments-1].y = i/image->width;
 161                         //a umistime souradnice pixelu na vrchol zasobniku
 162                         stack[stackPosition++] = i;
 163                         //dokud neni zasobnik prazdny
 164                         while (stackPosition > 0){
 165                                 //vyjmeme ze zasobniku pozici posledne vlozeneho pixelu
 166                                 position = stack[--stackPosition];
 167                                 //prohledame pixely na sousednich pozicich
 168                                 for (int j =0;j<4;j++){
 169                                         pos = position+expand[j];
 170                                         //a pokud maji hledanou barvu,
 171                                         if (buffer[pos] < 0){
 172                                                 //vlozime je do zasobniku,
 173                                                 stack[stackPosition++] = pos;
 174                                                 //pridame jejich pozici do souradnic aktualniho segmentu
 175                                                 segmentArray[numSegments-1].x += pos%image->width;
 176                                                 segmentArray[numSegments-1].y += pos/image->width;
 177                                                 //a zvetsime velikost segmentu
 178                                                 segmentArray[numSegments-1].size++;
 179                                                 buffer[pos] = numSegments;
 180                                         }
 181                                 }
 182                         }
 183                         //jakmile se zasobnik vyprazdni, tj. byly nalezeny vsechny pisely daneho segmentu  spocteme teziste segmentu
 184                         segmentArray[numSegments-1].x = segmentArray[numSegments-1].x/segmentArray[numSegments-1].size;
 185                         segmentArray[numSegments-1].y = segmentArray[numSegments-1].y/segmentArray[numSegments-1].size;
 186                 }
 187         }
 188
 189         //Najde nejvetsi segment
 190         int maxSize = 0;
 191         int index = 0;
 192         int i;
 193         for (i =0;i<numSegments;i++){
 194                 if (maxSize < segmentArray[i].size){
 195                         index = i;
 196                         maxSize = segmentArray[i].size;
 197                 }
 198         }
 199         if (debug) fprintf(stdout,"Largest segment is %i %i %i %i\n",index,segmentArray[index].size,segmentArray[index].x,segmentArray[index].y);
 200
 201         //a spocte jeho stred
 202         if (maxSize > 20){
 203                 result.x = segmentArray[index].x;
 204                 result.y = segmentArray[index].y;
 205                 result.info = true;
 206                 //fprintf(stdout,"xy = %i %i\n", result.x, result.y);
 207         }
 208
 209         //vykreslime vysledek
 210         int j = 0;
 211         for (int i = 0;i<len;i++){
 212                 j = buffer[i];
 213                 if (j > 0){
 214                         image->data[i*3+j%3] = 0;
 215                         image->data[i*3+(j+1)%3] = 255;
 216                         image->data[i*3+(j+2)%3] = 255;
 217                 }
 218
 219         }
 220
 221         return result;
 222 }
 223
 224 //nalezeni teziste pixelu dane tridy - metoda z dilu III
 225 SPixelPosition CRecognition::findMean(CRawImage* image)
 226 {
 227         //priprava promennych pro vypocet
 228         SPixelPosition result;
 229         float sumX,sumY,eval,sumEval;
 230         sumX=sumY=eval=sumEval=0;
 231         int step = 1;
 232         int yconst = image->width;
 233
 234         //vlastni vypocet
 235         for (int y = 0;y<image->height;y+=step){
 236                 yconst = image->width*y;
 237                 for (int x = 0;x<image->width;x+=step){
 238                         //vyhodnoceni podobnosti pixelu
 239                         eval=evaluatePixelFast(&image->data[3*(x+yconst)]);
 240                         //vybarveni vysledku v obraze
 241                         if (eval == 1){
 242                                 for (int i = 0;i<3;i++) image->data[3*(x+yconst)+i]=255-learned[i];
 243                         }
 244                         sumEval +=eval;
 245                         sumX = sumX + x*eval;
 246                         sumY = sumY + y*eval;
 247                 }
 248         }
 249         //pokud byl nalezen alespon jeden pixel, je proveden vypocet teziste
 250         if (sumEval > 0){
 251                 sumX = sumX/sumEval;
 252                 sumY = sumY/sumEval;
 253         }else{
 254                 sumX = image->width/2;
 255                 sumY = image->height/2;
 256         }
 257         //vypocet stredu
 258         result.x = (int)(sumX);
 259         result.y = (int)(sumY);
 260         //fprintf(stdout,"x2y2 = %i %i\n", result.x, result.y);
 261         return result;
 262 }
 263
 264 //prevod RGB -> HSV, prevzato z www
 265 void CRecognition::rgbToHsv(unsigned char r, unsigned char  g, unsigned char b, unsigned int *hue, unsigned char *saturation, unsigned char *value )
 266 {
 267         float min, max, delta;
 268         float h,s,v;
 269
 270         h=s=v=0;
 271         *saturation = (unsigned char) s;
 272         *value = (unsigned char) v;
 273         *hue = (unsigned int) h;
 274
 275         min = min( r, min(g, b) );
 276         max = max( r, max(g, b) );
 277         v = max;
 278
 279         delta = max - min;
 280
 281         if( max != 0 )
 282                 s = min(delta*255 / max,255);
 283         else {
 284                 s = 0;
 285                 h = -1;
 286                 return;
 287         }
 288
 289         if( r == max )
 290                 h = ( g - b ) / delta;          // between yellow & magenta
 291         else if( g == max )
 292                 h = 2 + ( b - r ) / delta;      // between cyan & yellow
 293         else
 294                 h = 4 + ( r - g ) / delta;      // between magenta & cyan
 295         h = h*60;
 296         if (h<0) h+=360;
 297         *saturation = (unsigned char) s;
 298         *value = (unsigned char) v;
 299         *hue = (unsigned int) h;
 300 }
 301