garfeild/keypoints/yape.cpp

   1 /*
   2 Copyright 2005, 2006 Computer Vision Lab,
   3 Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
   4 All rights reserved.
   5
   6 This file is part of BazAR.
   7
   8 BazAR is free software; you can redistribute it and/or modify it under the
   9 terms of the GNU General Public License as published by the Free Software
  10 Foundation; either version 2 of the License, or (at your option) any later
  11 version.
  12
  13 BazAR is distributed in the hope that it will be useful, but WITHOUT ANY
  14 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
  15 PARTICULAR PURPOSE. See the GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License along with
  18 BazAR; if not, write to the Free Software Foundation, Inc., 51 Franklin
  19 Street, Fifth Floor, Boston, MA 02110-1301, USA
  20 */
  21 /*
  22 * Author:
  23 * Vincent Lepetit <vincent.lepetit@epfl.ch>
  24 * 2004/2005
  25 */
  26
  27 #include <algorithm>
  28 #include <iostream>
  29 #include <fstream>
  30
  31 using namespace std;
  32
  33 #include <stdlib.h>
  34 #include <math.h>
  35
  36 #include <starter.h>
  37 #include "yape.h"
  38
  39 #include "../../artvertiser/FProfiler/FProfiler.h"
  40
  41 const unsigned int yape_bin_size = 1000;
  42 const int yape_tmp_points_array_size = 10000;
  43
  44 bool operator <(const keypoint &p1, const keypoint &p2)
  45 {
  46   return p1.score > p2.score;
  47 }
  48
  49 yape::yape(int _width, int _height)
  50 {
  51   width = _width;
  52   height = _height;
  53
  54   radius = 7;
  55   tau = 10;
  56   minimal_neighbor_number = 4;
  57
  58   activate_bins();
  59   set_bins_number(4, 4);
  60
  61   disactivate_subpixel();
  62
  63   set_minimal_neighbor_number(3);
  64
  65   init_for_monoscale();
  66 }
  67
  68 yape::~yape()
  69 {
  70   if (Dirs) delete Dirs;
  71   if (Dirs_nb) delete[] Dirs_nb;
  72   if (scores) cvReleaseImage(&scores);
  73   if (filtered_image) cvReleaseImage(&filtered_image);
  74   if (raw_detect_thread_data.size()>0)
  75   {
  76       // cleanup threads
  77       for ( int i=0; i<raw_detect_thread_data.size(); i++ )
  78       {
  79           delete raw_detect_thread_data[i];
  80       }
  81       raw_detect_thread_data.clear();
  82       delete shared_barrier;
  83   }
  84 }
  85
  86 int yape::static_detect(IplImage * image, keypoint * points, int max_point_number, int _radius, int _tau)
  87 {
  88   yape * pe = new yape(image->width, image->height);
  89
  90   pe->set_radius(_radius);
  91   pe->set_tau(_tau);
  92   int point_number = pe->detect(image, points, max_point_number);
  93
  94   delete pe;
  95
  96   return point_number;
  97 }
  98
  99 void yape::set_radius(int _radius)
 100 {
 101   radius = _radius;
 102 }
 103
 104 void yape::set_tau(int _tau)
 105 {
 106   tau = _tau;
 107 }
 108
 109 void yape::init_for_monoscale(void)
 110 {
 111   scores = cvCreateImage(cvSize(width, height), IPL_DEPTH_16S, 1);
 112   filtered_image = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
 113
 114   Dirs = new dir_table;
 115   memset(Dirs,0,sizeof(dir_table));
 116   Dirs_nb = new int[yape_max_radius];
 117   memset(Dirs_nb,0,sizeof(int)*yape_max_radius);
 118   for(int R = 1; R < yape_max_radius; R++)
 119     precompute_directions(filtered_image, Dirs->t[R], &(Dirs_nb[R]), R);
 120 }
 121
 122 void yape::reserve_tmp_arrays(void)
 123 {
 124   if (use_bins)
 125     for(int i = 0; i < bin_nb_u; i++)
 126       for(int j = 0; j < bin_nb_v; j++)
 127       {
 128         bins[i][j].clear();
 129         bins[i][j].reserve(yape_bin_size);
 130       }
 131   else
 132   {
 133     tmp_points.clear();
 134     tmp_points.reserve(yape_tmp_points_array_size);
 135   }
 136 }
 137
 138 void yape::precompute_directions(IplImage * image, short * _Dirs, int * _Dirs_nb, int R)
 139 {
 140   int widthStep = image->widthStep;
 141
 142   int i = 0;
 143   int x, y;
 144
 145   x = R;
 146   for(y = 0; y < x; y++, i++)
 147   {
 148     x = int(sqrt(double(R * R - y * y)) + 0.5);
 149     _Dirs[i] = short(x + widthStep * y);
 150   }
 151   for(x-- ; x < y && x >= 0; x--, i++)
 152   {
 153     y = int(sqrt(double(R * R - x * x)) + 0.5);
 154     _Dirs[i] = short(x + widthStep * y);
 155   }
 156   for( ; -x < y; x--, i++)
 157   {
 158     y = int(sqrt(double(R * R - x * x)) + 0.5);
 159     _Dirs[i] = short(x + widthStep * y);
 160   }
 161   for(y-- ; y >= 0; y--, i++)
 162   {
 163     x = int(-sqrt(double(R * R - y * y)) - 0.5);
 164     _Dirs[i] = short(x + widthStep * y);
 165   }
 166   for(; y > x; y--, i++)
 167   {
 168     x = int(-sqrt(double(R * R - y * y)) - 0.5);
 169     _Dirs[i] = short(x + widthStep * y);
 170   }
 171   for(x++ ; x <= 0; x++, i++)
 172   {
 173     y = int(-sqrt(double(R * R - x * x)) - 0.5);
 174     _Dirs[i] = short(x + widthStep * y);
 175   }
 176   for( ; x < -y; x++, i++)
 177   {
 178     y = int(-sqrt(double(R * R - x * x)) - 0.5);
 179     _Dirs[i] = short(x + widthStep * y);
 180   }
 181   for(y++ ; y < 0; y++, i++)
 182   {
 183     x = int(sqrt(double(R * R - y * y)) + 0.5);
 184     _Dirs[i] = short(x + widthStep * y);
 185   }
 186
 187   *_Dirs_nb = i;
 188   _Dirs[i] = _Dirs[0];
 189   _Dirs[i + 1] = _Dirs[1];
 190 }
 191
 192 void yape::save_image_of_detected_points(char * name, IplImage * image, keypoint * points, int points_nb)
 193 {
 194   IplImage * color_image = mcvGrayToColor(image);
 195
 196   for(int i = 0; i < points_nb; i++) {
 197     int s = (int)points[i].scale;
 198     float x = PyrImage::convCoordf(points[i].u, s, 0);
 199     float y = PyrImage::convCoordf(points[i].v, s, 0);
 200     float l = PyrImage::convCoordf(32.0f, s, 0);
 201
 202     mcvCircle(color_image, int(x), int(y), (int)l, mcvRainbowColor(s % 7));
 203   }
 204
 205   mcvSaveImage(name, color_image);
 206 }
 207
 208 bool yape::double_check(IplImage * image, int x, int y, short * dirs, unsigned char dirs_nb)
 209 {
 210   unsigned char * I = (unsigned char *)(image->imageData + y * image->widthStep);
 211   int Ip = I[x] + tau;
 212   int Im = I[x] - tau;
 213   int A;
 214
 215   for(A = dirs_nb / 2 - 2; A <= dirs_nb / 2 + 2; A++)
 216     for(int i = 0; i < dirs_nb - A; i++)
 217     {
 218       if (I[x+dirs[i]] > Im && I[x+dirs[i]] < Ip && I[x+dirs[i+A]] > Im && I[x+dirs[i+A]] < Ip)
 219         return false;
 220     }
 221
 222     return true;
 223 }
 224
 225 #define A_INF (A < Im)
 226 #define A_SUP (A > Ip)
 227 #define A_NOT_INF (A >= Im)
 228 #define A_NOT_SUP (A <= Ip)
 229
 230 #define B0_INF (B0 < Im)
 231 #define B0_SUP (B0 > Ip)
 232 #define B0_NOT_INF (B0 >= Im)
 233 #define B0_NOT_SUP (B0 <= Ip)
 234
 235 #define B1_INF (B1 < Im)
 236 #define B1_SUP (B1 > Ip)
 237 #define B1_NOT_INF (B1 >= Im)
 238 #define B1_NOT_SUP (B1 <= Ip)
 239
 240 #define B2_INF (B2 < Im)
 241 #define B2_SUP (B2 > Ip)
 242 #define B2_NOT_INF (B2 >= Im)
 243 #define B2_NOT_SUP (B2 <= Ip)
 244
 245 #define GET_A_old()   A = I[x+dirs[a]]
 246 #define GET_B0_old()  B0 = I[x+dirs[b]]
 247 #define GET_B1_old()  b++; B1 = I[x+dirs[b]]
 248 #define GET_B2_old()  b++; B2 = I[x+dirs[b]]
 249 #define GET_A_paranoid()   temp = cache[a]; GET_A_old(); printf("GET_A: %i %i %i\n", temp, a, A ); assert(temp==A)
 250 #define GET_B0_paranoid()  temp = cache[b]; GET_B0_old(); printf("GET_B0: %i %i %i\n", temp, b, B0 ); assert(temp==B0)
 251 #define GET_B1_paranoid()  temp = cache[++b]; --b; GET_B1_old(); printf("GET_B1: %i %i %i\n", temp, b, B1 ); assert(temp==B1)
 252 #define GET_B2_paranoid()  temp = cache[++b]; --b; GET_B2_old(); printf("GET_B2: %i %i %i\n", temp, b, B2 ); assert(temp==B2)
 253 #define GET_A_new()  A = cache[a];
 254 #define GET_B0_new() B0 = cache[b];
 255 #define GET_B1_new() B1 = cache[++b];
 256 #define GET_B2_new() B2 = cache[++b];
 257
 258 #define GET_A()  GET_A_new()
 259 #define GET_B0() GET_B0_new()
 260 #define GET_B1() GET_B1_new()
 261 #define GET_B2() GET_B2_new()
 262
 263 #define GOTO_STATE(s)    { score -= A + B1; state = (s); break; }
 264 #define GOTO_END_NOT_AN_INTEREST_POINT { /*stats_iter[a]++;*/ Scores[x] = 0; return; }
 265 #define PUT_B2_IN_B1_AND_GET_B2()  B1 = B2; GET_B2()
 266
 267 #define B1_NOT_INF_B2_NOT_INF  0
 268 #define B1_NOT_SUP_B2_NOT_SUP  1
 269 #define B1_INF_B2_INF          2
 270 #define B1_SUP_B2_SUP          3
 271 #define B1_INF_B2_NOT_SUP      4
 272 #define B1_SUP_B2_NOT_INF      5
 273 #define B1_SUP_B2_INF          6
 274 #define B1_INF_B2_SUP          7
 275 #define B1_EQUAL_B2_NOT_SUP    8
 276 #define B1_EQUAL_B2_NOT_INF    9
 277
 278 void yape::perform_one_point(const unsigned char * I, const int x, short * Scores,
 279                              const int Im, const int Ip,
 280                              const short * dirs, const unsigned char opposite, const unsigned char dirs_nb)
 281 {
 282
 283   int score = 0;
 284   unsigned char a = 0, b = opposite - 1;
 285   unsigned char A, B0, B1, B2;
 286   unsigned char state;
 287   unsigned char temp;
 288
 289   unsigned char cache[dirs_nb];
 290   for ( int i=0; i<dirs_nb; i++ )
 291   {
 292       cache[i] = I[x+dirs[i]];
 293   }
 294
 295   /*printf("dirs: opp %hi, ", (short)opposite );
 296   for ( int i=0; i<dirs_nb; i++ )
 297   {
 298       printf("%hi ", dirs[i] );
 299   }
 300   printf("\n");*/
 301
 302   /*GET_A();
 303   GET_B0();
 304   GET_B1();
 305   GET_B2();
 306
 307   if ( A_NOT_SUP )*/
 308
 309
 310   // WE KNOW THAT NOT(A ~ I0 & B1 ~ I0):
 311   GET_A();
 312   if (A_NOT_SUP) {
 313     if (A_NOT_INF) { // A ~ I0
 314       GET_B0();
 315       if (B0_NOT_SUP) {
 316         if (B0_NOT_INF) GOTO_END_NOT_AN_INTEREST_POINT
 317         else {
 318           GET_B1();
 319           if (B1_SUP) {
 320             GET_B2();
 321             if (B2_SUP) state = B1_SUP_B2_SUP;
 322             else if (B2_INF) state = B1_SUP_B2_INF;
 323             else GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B2 ~ I0
 324           }
 325           else/* if (B1_INF)*/ {
 326             GET_B2();
 327             if (B2_SUP) state = B1_INF_B2_SUP;
 328             else if (B2_INF) state = B1_INF_B2_INF;
 329             else GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B2 ~ I0
 330           }
 331           //else GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B1 ~ I0
 332         }
 333       }
 334       else { // B0 < I0
 335         GET_B1();
 336         if (B1_SUP) {
 337           GET_B2();
 338           if (B2_SUP) state = B1_SUP_B2_SUP;
 339           else if (B2_INF) state = B1_SUP_B2_INF;
 340           else GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B2 ~ I0
 341         }
 342         else if (B1_INF) {
 343           GET_B2();
 344           if (B2_SUP) state = B1_INF_B2_SUP;
 345           else if (B2_INF) state = B1_INF_B2_INF;
 346           else GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B2 ~ I0
 347         }
 348         else GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B1 ~ I0
 349       }
 350     }
 351     else { // A > I0
 352       GET_B0();
 353       if (B0_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 354         GET_B1();
 355       if (B1_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 356         GET_B2();
 357       if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 358         state = B1_NOT_SUP_B2_NOT_SUP;
 359     }
 360   }
 361   else // A < I0
 362   {
 363     GET_B0();
 364     if (B0_INF) GOTO_END_NOT_AN_INTEREST_POINT
 365       GET_B1();
 366     if (B1_INF) GOTO_END_NOT_AN_INTEREST_POINT
 367       GET_B2();
 368     if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 369       state = B1_NOT_INF_B2_NOT_INF;
 370   }
 371
 372   for(a = 1; a <= opposite; a++)
 373   {
 374     GET_A();
 375
 376     switch(state)
 377     {
 378     case B1_NOT_INF_B2_NOT_INF:
 379       if (A_SUP) {
 380         PUT_B2_IN_B1_AND_GET_B2();
 381         if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 382           GOTO_STATE(B1_NOT_INF_B2_NOT_INF);
 383       }
 384       if (A_INF) {
 385         if (B1_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 386           if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 387             PUT_B2_IN_B1_AND_GET_B2();
 388         if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 389           GOTO_STATE(B1_EQUAL_B2_NOT_SUP);
 390       }
 391       // A ~ I0
 392       if (B1_NOT_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 393         if (B2_NOT_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 394           PUT_B2_IN_B1_AND_GET_B2();
 395       if (B2_SUP) GOTO_STATE(B1_SUP_B2_SUP);
 396       if (B2_INF) GOTO_STATE(B1_SUP_B2_INF);
 397       GOTO_END_NOT_AN_INTEREST_POINT
 398
 399     case B1_NOT_SUP_B2_NOT_SUP:
 400       if (A_INF) {
 401         PUT_B2_IN_B1_AND_GET_B2();
 402         if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 403           GOTO_STATE(B1_NOT_SUP_B2_NOT_SUP);
 404       }
 405       if (A_SUP) {
 406         if (B1_INF) GOTO_END_NOT_AN_INTEREST_POINT
 407           if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 408             PUT_B2_IN_B1_AND_GET_B2();
 409         if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 410           GOTO_STATE(B1_EQUAL_B2_NOT_INF);
 411       }
 412       // A ~ I0
 413       if (B1_NOT_INF) GOTO_END_NOT_AN_INTEREST_POINT
 414         if (B2_NOT_INF) GOTO_END_NOT_AN_INTEREST_POINT
 415           PUT_B2_IN_B1_AND_GET_B2();
 416       if (B2_INF) GOTO_STATE(B1_INF_B2_INF);
 417       if (B2_SUP) GOTO_STATE(B1_INF_B2_SUP);
 418       GOTO_END_NOT_AN_INTEREST_POINT
 419
 420     case B1_INF_B2_INF:
 421       if (A_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 422         PUT_B2_IN_B1_AND_GET_B2();
 423       if (A_INF)
 424       {
 425         if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 426           GOTO_STATE(B1_INF_B2_NOT_SUP);
 427       }
 428       // A ~ I0
 429       if (B2_SUP) GOTO_STATE(B1_INF_B2_SUP);
 430       if (B2_INF) GOTO_STATE(B1_INF_B2_INF);
 431       GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B2 ~ I0
 432
 433     case B1_SUP_B2_SUP:
 434       if (A_INF) GOTO_END_NOT_AN_INTEREST_POINT
 435         PUT_B2_IN_B1_AND_GET_B2();
 436       if (A_SUP) {
 437         if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 438           GOTO_STATE(B1_SUP_B2_NOT_INF);
 439       }
 440       // A ~ I0
 441       if (B2_SUP) GOTO_STATE(B1_SUP_B2_SUP);
 442       if (B2_INF) GOTO_STATE(B1_SUP_B2_INF);
 443       GOTO_END_NOT_AN_INTEREST_POINT
 444
 445     case B1_INF_B2_NOT_SUP:
 446       if (A_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 447         if (A_INF) {
 448           PUT_B2_IN_B1_AND_GET_B2();
 449           if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 450             GOTO_STATE(B1_NOT_SUP_B2_NOT_SUP);
 451         }
 452         if (B2_NOT_INF) GOTO_END_NOT_AN_INTEREST_POINT
 453           PUT_B2_IN_B1_AND_GET_B2();
 454         if (B2_INF) GOTO_STATE(B1_INF_B2_INF);
 455         if (B2_SUP) GOTO_STATE(B1_INF_B2_SUP);
 456         GOTO_END_NOT_AN_INTEREST_POINT
 457
 458     case B1_SUP_B2_NOT_INF:
 459       if (A_INF) GOTO_END_NOT_AN_INTEREST_POINT
 460         if (A_SUP) {
 461           PUT_B2_IN_B1_AND_GET_B2();
 462           if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 463             GOTO_STATE(B1_NOT_INF_B2_NOT_INF);
 464         }
 465         // A ~ I0
 466         if (B2_NOT_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 467           PUT_B2_IN_B1_AND_GET_B2();
 468         if (B2_SUP) GOTO_STATE(B1_SUP_B2_SUP);
 469         if (B2_INF) GOTO_STATE(B1_SUP_B2_INF);
 470         GOTO_END_NOT_AN_INTEREST_POINT
 471
 472     case B1_INF_B2_SUP:
 473       if (A_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 474         if (A_INF) GOTO_END_NOT_AN_INTEREST_POINT
 475           PUT_B2_IN_B1_AND_GET_B2();
 476       // A ~ I0
 477       if (B2_SUP) GOTO_STATE(B1_SUP_B2_SUP);
 478       if (B2_INF) GOTO_STATE(B1_SUP_B2_INF);
 479       GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B2 ~ I0
 480
 481     case B1_SUP_B2_INF:
 482       if (A_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 483         if (A_INF) GOTO_END_NOT_AN_INTEREST_POINT
 484           PUT_B2_IN_B1_AND_GET_B2();
 485       // A ~ I0
 486       if (B2_INF) GOTO_STATE(B1_INF_B2_INF);
 487       if (B2_SUP) GOTO_STATE(B1_INF_B2_SUP);
 488       GOTO_END_NOT_AN_INTEREST_POINT // A ~ I0, B2 ~ I0
 489
 490     case B1_EQUAL_B2_NOT_SUP:
 491       if (A_SUP) {
 492         if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 493           PUT_B2_IN_B1_AND_GET_B2();
 494         if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 495           GOTO_STATE(B1_EQUAL_B2_NOT_INF);
 496       }
 497       if (A_INF) {
 498         PUT_B2_IN_B1_AND_GET_B2();
 499         if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 500           GOTO_STATE(B1_NOT_SUP_B2_NOT_SUP);
 501       }
 502       GOTO_END_NOT_AN_INTEREST_POINT
 503
 504     case B1_EQUAL_B2_NOT_INF:
 505       if (A_INF) {
 506         if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 507           PUT_B2_IN_B1_AND_GET_B2();
 508         if (B2_SUP) GOTO_END_NOT_AN_INTEREST_POINT
 509           GOTO_STATE(B1_EQUAL_B2_NOT_SUP);
 510       }
 511       if (A_SUP) {
 512         PUT_B2_IN_B1_AND_GET_B2();
 513         if (B2_INF) GOTO_END_NOT_AN_INTEREST_POINT
 514           GOTO_STATE(B1_NOT_INF_B2_NOT_INF);
 515       }
 516       GOTO_END_NOT_AN_INTEREST_POINT
 517
 518     default:
 519       cout << "PB default" << endl;
 520     } // switch(state)
 521   } // for(a...)
 522
 523   Scores[x] = short(score + dirs_nb * I[x]);
 524 }
 525
 526 int yape::detect(IplImage * image, keypoint * points, int max_point_number, IplImage * _filtered_image)
 527 {
 528   IplImage * used_filtered_image;
 529
 530   if (_filtered_image == 0)
 531   {
 532     int gaussian_filter_size = 3;
 533
 534     if (radius >= 5) gaussian_filter_size = 5;
 535     if (radius >= 7) gaussian_filter_size = 7;
 536     cvSmooth(image, filtered_image, CV_GAUSSIAN, gaussian_filter_size, gaussian_filter_size);
 537
 538     used_filtered_image = filtered_image;
 539   }
 540   else
 541     used_filtered_image = _filtered_image;
 542
 543   reserve_tmp_arrays();
 544
 545   raw_detect_mt(used_filtered_image);
 546
 547   get_local_maxima(image, radius, 0);
 548
 549   int points_nb = pick_best_points(points, max_point_number);
 550
 551   if (use_subpixel)
 552     for(int i = 0; i < points_nb; i++)
 553       subpix_refine(used_filtered_image, points + i);
 554
 555   return points_nb;
 556 }
 557
 558 static unsigned mymin(unsigned a, unsigned b)
 559 {
 560         return (a<b ? a:b);
 561 }
 562
 563 /*! This method sorts tmp_points and select the max_point_number best points,
 564 * as long as the score is high enough.
 565 */
 566 int yape::pick_best_points(keypoint * points, unsigned int max_point_number)
 567 {
 568   if (use_bins)
 569   {
 570     unsigned int N = 0;
 571     for(int i = 0; i < bin_nb_u; i++)
 572     {
 573       for(int j = 0; j < bin_nb_v; j++)
 574       {
 575         if (bins[i][j].size() > 0){
 576           sort(bins[i][j].begin(), bins[i][j].end());
 577           N += bins[i][j].size();
 578         }
 579       }
 580     }
 581
 582     N = max_point_number;
 583     unsigned int N_per_bin = N / (bin_nb_u * bin_nb_v);
 584     int points_nb = 0;
 585     for(int i = 0; i < bin_nb_u; i++)
 586     {
 587       for(int j = 0; j < bin_nb_v; j++)
 588       {
 589         for(unsigned int k = 0; k < mymin(N_per_bin, bins[i][j].size()); k++, points_nb++)
 590           points[points_nb] = bins[i][j][k];
 591       }
 592     }
 593
 594     return points_nb;
 595   }
 596   else
 597     if (tmp_points.size() > 0)
 598     {
 599       sort(tmp_points.begin(), tmp_points.end());
 600
 601       int score_threshold = 0;
 602
 603       int tot_pts = tmp_points.size()<max_point_number ? tmp_points.size() : max_point_number;
 604
 605       int points_nb = 0;
 606       for(points_nb = 0;
 607         points_nb<tot_pts && tmp_points[points_nb].score>score_threshold;
 608         points_nb++)
 609         points[points_nb] = tmp_points[points_nb];
 610
 611       return points_nb;
 612     }
 613
 614     return 0;
 615 }
 616
 617
 618 static inline int computeLResponse(const uchar* ptr, const short* cdata, int csize)
 619 {
 620     int i, csize2 = csize/2, sum = -ptr[0]*csize;
 621     for( i = 0; i < csize2; i++ )
 622     {
 623         int ofs = cdata[i];
 624         sum += ptr[ofs] + ptr[-ofs];
 625     }
 626     return abs(sum);
 627 }
 628
 629 static inline void perform_one_point_2( const unsigned char* img, int x, short* scores, const int tau,
 630                          const short* dirs, const unsigned char opposite, const unsigned char dirs_nb)
 631 {
 632
 633 /*
 634 void yape::perform_one_point(const unsigned char * I, const int x, short * Scores,
 635                              const int Im, const int Ip,
 636                              const short * dirs, const unsigned char opposite, const unsigned char dirs_nb)
 637 {
 638
 639   int score = 0;
 640   unsigned char a = 0, b = opposite - 1;
 641   unsigned char A, B0, B1, B2;
 642   unsigned char state;
 643   unsigned char temp;
 644
 645   unsigned char cache[dirs_nb];
 646   for ( int i=0; i<dirs_nb; i++ )
 647   {
 648       cache[i] = I[x+dirs[i]];
 649   }
 650 */
 651     short val0 = *img;
 652     unsigned char k;
 653     for( k = 0; k < dirs_nb; k++ )
 654     {
 655         if( abs(val0 - img[dirs[k]]) < tau &&
 656            (abs(val0 - img[dirs[k + opposite]]) < tau ||
 657             abs(val0 - img[dirs[k + opposite - 1]]) < tau ||
 658             abs(val0 - img[dirs[k + opposite + 1]]) < tau ||
 659             abs(val0 - img[dirs[k + opposite - 2]]) < tau ||
 660             abs(val0 - img[dirs[k + opposite + 2]]) < tau ) )
 661             break;
 662     }
 663
 664     if( k < dirs_nb )
 665     {
 666         scores[x] = 0;
 667     }
 668     else
 669     {
 670         scores[x] = (short)computeLResponse(img, dirs, dirs_nb);
 671     }
 672 }
 673
 674 yape::RawDetectThreadData::RawDetectThreadData()
 675 {
 676     run_semaphore = new FSemaphore( 0 );
 677 }
 678
 679 yape::RawDetectThreadData::~RawDetectThreadData()
 680 {
 681     // kill the thread
 682     should_stop=true;
 683     run_semaphore->Signal();
 684     void* ret;
 685     pthread_join( thread, &ret );
 686     delete run_semaphore;
 687 }
 688
 689 void* yape::raw_detect_thread_func( void* _data )
 690 {
 691     RawDetectThreadData* data = (RawDetectThreadData*)_data;
 692
 693     while ( true )
 694     {
 695         // continue/stop mechanism
 696         data->run_semaphore->Wait();
 697         if ( data->should_stop )
 698             break;
 699
 700         unsigned int R = data->y->radius;
 701         short * dirs = data->y->Dirs->t[R];
 702         unsigned char dirs_nb = (unsigned char)(data->y->Dirs_nb[R]);
 703         unsigned char opposite = dirs_nb / 2;
 704
 705         CvRect roi = cvGetImageROI(data->im);
 706
 707         if (roi.x < int(R+1)) roi.x = R+1;
 708         if (roi.y < int(R+1)) roi.y = R+1;
 709         if ((roi.x + roi.width)  > int(data->im->width-R-2))  roi.width  = data->im->width  - R - roi.x - 2;
 710         if ((roi.y + roi.height) > int(data->im->height-R-2)) roi.height = data->im->height - R - roi.y - 2;
 711
 712         unsigned int xend = roi.x + roi.width;
 713         unsigned int yend = data->y_start + data->y_count;
 714         int tau = data->y->tau;
 715
 716         for(unsigned int y = data->y_start; y < yend; y++)
 717         {
 718             unsigned char* I = (unsigned char*)(data->im->imageData + y*data->im->widthStep);
 719             short * Scores = (short*)(data->y->scores->imageData + y*data->y->scores->widthStep);
 720             for(unsigned int x = roi.x; x < xend; x++)
 721             {
 722                 unsigned char* img = I+x;
 723                 int Ip = I[x] + tau;
 724                 int Im = I[x] - tau;
 725
 726                 if (Im<img[R] && img[R]<Ip && Im<img[-R] && img[-R]<Ip)
 727                     Scores[x] = 0;
 728                 else
 729                 {
 730                     //perform_one_point(I, x, Scores, Im, Ip, dirs, opposite, dirs_nb);
 731                     //int old_score = Scores[x];
 732                     perform_one_point_2( img, x, Scores, tau, dirs, opposite, dirs_nb );
 733                     //int new_score = Scores[x];
 734                     //printf("%3i %3i: %i %i\n", x, y, old_score, new_score );
 735                 }
 736             }
 737         }
 738
 739         // finished
 740         data->barrier->Wait();
 741
 742     }
 743
 744     pthread_exit(0);
 745 }
 746
 747 void yape::raw_detect_mt(IplImage* im)
 748 {
 749     PROFILE_THIS_FUNCTION();
 750     int thread_count = 8;
 751     if ( raw_detect_thread_data.size() != thread_count )
 752     {
 753         assert( raw_detect_thread_data.size() == 0 );
 754
 755         // make shared barrier
 756         shared_barrier = new FBarrier( thread_count+1 );
 757
 758         // make threads joinable
 759         pthread_attr_t thread_attr;
 760         pthread_attr_init(&thread_attr);
 761         pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_JOINABLE);
 762         printf("creating %i raw_detect threads\n", thread_count);
 763         for ( int i=0; i<thread_count; i++ )
 764         {
 765             // setup data
 766             RawDetectThreadData* thread_data = new RawDetectThreadData;
 767             thread_data->run_semaphore = new FSemaphore(0);
 768             thread_data->y = this;
 769             thread_data->barrier = shared_barrier;
 770             thread_data->should_stop = false;
 771             // start the thread
 772             pthread_create( &thread_data->thread, &thread_attr, raw_detect_thread_func, (void*)thread_data );
 773             // store
 774             raw_detect_thread_data.push_back( thread_data );
 775         }
 776         pthread_attr_destroy(&thread_attr);
 777
 778     }
 779
 780     // go
 781     unsigned int R = radius;
 782     CvRect roi = cvGetImageROI(im);
 783     // get y for divisions
 784     if (roi.y < int(R+1)) roi.y = R+1;
 785     if ((roi.y + roi.height) > int(im->height-R-2)) roi.height = im->height - R - roi.y - 2;
 786
 787     // distribute y over threads
 788     int curr_y = roi.y;
 789     int y_remaining_count = roi.height;
 790     int y_count = y_remaining_count / raw_detect_thread_data.size();
 791     assert( y_remaining_count >= raw_detect_thread_data.size() && "too many threads / not enough roi.y height" );
 792     // start each thread
 793     for ( int i=0; i<raw_detect_thread_data.size(); i++ )
 794     {
 795         // get thread data
 796         RawDetectThreadData* thread_data = raw_detect_thread_data[i];
 797         thread_data->im = im;
 798         thread_data->y_start = curr_y;
 799         // distribute y ranges to trheads, ensuring last thread gets remainder
 800         if ( i== raw_detect_thread_data.size()-1 )
 801             thread_data->y_count = y_remaining_count;
 802         else
 803             thread_data->y_count = y_count;
 804         curr_y += y_count;
 805         y_remaining_count -= y_count;
 806
 807         // go!
 808         thread_data->run_semaphore->Signal();
 809
 810     }
 811     // wait for all threads to complete
 812     shared_barrier->Wait();
 813
 814 }
 815
 816 /*! Detect interest points, without filtering and without selecting best ones.
 817 * Just find them and add them to tmp_points. tmp_points is not cleared in this
 818 * method.
 819 */
 820 void yape::raw_detect(IplImage *im) {
 821   unsigned int R = radius;
 822   short * dirs = Dirs->t[R];
 823   unsigned char dirs_nb = (unsigned char)(Dirs_nb[R]);
 824   unsigned char opposite = dirs_nb / 2;
 825
 826   CvRect roi = cvGetImageROI(im);
 827
 828   if (roi.x < int(R+1)) roi.x = R+1;
 829   if (roi.y < int(R+1)) roi.y = R+1;
 830   if ((roi.x + roi.width)  > int(im->width-R-2))  roi.width  = im->width  - R - roi.x - 2;
 831   if ((roi.y + roi.height) > int(im->height-R-2)) roi.height = im->height - R - roi.y - 2;
 832
 833   unsigned int xend = roi.x + roi.width;
 834   unsigned int yend = roi.y + roi.height;
 835
 836     PROFILE_SECTION_PUSH("performing points");
 837
 838     for(unsigned int y = roi.y; y < yend; y++)
 839     {
 840         unsigned char* I = (unsigned char*)(im->imageData + y*im->widthStep);
 841         short * Scores = (short*)(scores->imageData + y*scores->widthStep);
 842         for(unsigned int x = roi.x; x < xend; x++)
 843         {
 844             unsigned char* img = I+x;
 845             int Ip = I[x] + tau;
 846             int Im = I[x] - tau;
 847
 848             if (Im<img[R] && img[R]<Ip && Im<img[-R] && img[-R]<Ip)
 849                 Scores[x] = 0;
 850             else
 851             {
 852                 perform_one_point(I, x, Scores, Im, Ip, dirs, opposite, dirs_nb);
 853                 //int old_score = Scores[x];
 854                 //perform_one_point_2( img, x, Scores, tau, dirs, opposite, dirs_nb );
 855                 //int new_score = Scores[x];
 856                 //printf("%3i %3i: %i %i\n", x, y, old_score, new_score );
 857             }
 858         }
 859     }
 860     PROFILE_SECTION_POP();
 861 }
 862
 863 /*
 864         for( y = radius; y < layerSize.height - radius; y++ )
 865         {
 866             const uchar* img = pyrLayer.ptr<uchar>(y) + radius;
 867             short* scores = scoreLayer.ptr<short>(y);
 868             uchar* mask = maskLayer.ptr<uchar>(y);
 869
 870             for( x = 0; x < radius; x++ )
 871             {
 872                 scores[x] = scores[layerSize.width - 1 - x] = 0;
 873                 mask[x] = mask[layerSize.width - 1 - x] = 0;
 874             }
 875
 876             for( x = radius; x < layerSize.width - radius; x++, img++ )
 877             {
 878                 int val0 = *img;
 879                 if( (std::abs(val0 - img[radius]) < tau && std::abs(val0 - img[-radius]) < tau) ||
 880                    (std::abs(val0 - img[vradius]) < tau && std::abs(val0 - img[-vradius]) < tau))
 881                 {
 882                     scores[x] = 0;
 883                     mask[x] = 0;
 884                     continue;
 885                 }
 886
 887                 for( k = 0; k < csize; k++ )
 888                 {
 889                     if( std::abs(val0 - img[cdata[k]]) < tau &&
 890                        (std::abs(val0 - img[cdata[k + csize2]]) < tau ||
 891                         std::abs(val0 - img[cdata[k + csize2 - 1]]) < tau ||
 892                         std::abs(val0 - img[cdata[k + csize2 + 1]]) < tau ||
 893                         std::abs(val0 - img[cdata[k + csize2 - 2]]) < tau ||
 894                         std::abs(val0 - img[cdata[k + csize2 + 2]]) < tau ) )
 895                         break;
 896                 }
 897
 898                 if( k < csize )
 899                 {
 900                     scores[x] = 0;
 901                     mask[x] = 0;
 902                 }
 903                 else
 904                 {
 905                     scores[x] = (short)computeLResponse(img, cdata, csize);
 906                     mask[x] = 1;
 907                 }
 908             }
 909         }
 910 */
 911
 912 #define Dx 1
 913 #define Dy next_line
 914
 915 #define W (-Dx)
 916 #define E (+Dx)
 917 #define N (-Dy)
 918 #define S (+Dy)
 919
 920 #define MINIMAL_SCORE (0 / radius)
 921
 922 inline bool yape::third_check(const short * Sb, const int next_line)
 923 {
 924   int n = 0;
 925
 926   if (Sb[E]   != 0) n++;
 927   if (Sb[W]   != 0) n++;
 928   if (Sb[S]   != 0) n++;
 929   if (Sb[S+E] != 0) n++;
 930   if (Sb[S+W] != 0) n++;
 931   if (Sb[N]   != 0) n++;
 932   if (Sb[N+E] != 0) n++;
 933   if (Sb[N+W] != 0) n++;
 934
 935   return n >= minimal_neighbor_number;
 936 }
 937
 938 // Test if a pixel is a local maxima in a given neighborhood.
 939 static inline bool is_local_maxima(const short *p, int neighborhood, const IplImage *scores)
 940 {
 941 #ifdef CONSIDER_ABS_VALUE_ONLY
 942   unsigned v = abs(p[0]);
 943   if (v < 5) return false;
 944
 945   int step = scores->widthStep / 2;
 946
 947   p -= step*neighborhood;
 948   for (int y= -neighborhood; y<=neighborhood; ++y) {
 949     for (int x= -neighborhood; x<=neighborhood; ++x) {
 950       if (abs(p[x]) > v)
 951         return false;
 952     }
 953     p += step;
 954   }
 955   return true;
 956 #else
 957   int v = p[0];
 958
 959   int step = scores->widthStep / 2;
 960
 961   if (v > 0) {
 962     p -= step*neighborhood;
 963     for (int y= -neighborhood; y<=neighborhood; ++y) {
 964       for (int x= -neighborhood; x<=neighborhood; ++x) {
 965         if (p[x] > v)
 966           return false;
 967       }
 968       p += step;
 969     }
 970   } else {
 971     p -= step*neighborhood;
 972     for (int y= -neighborhood; y<=neighborhood; ++y) {
 973       for (int x= -neighborhood; x<=neighborhood; ++x) {
 974         if (p[x] < v)
 975           return false;
 976       }
 977       p += step;
 978     }
 979   }
 980
 981   return true;
 982
 983 #endif
 984 }
 985
 986 /*! Find local maximas in score image and append them to tmp_points.
 987 * \return the number of local maxima found.
 988 */
 989 int yape::get_local_maxima(IplImage * image, int R, float scale /*, keypoint * points, int max_number_of_points*/)
 990 {
 991   int nbpts=0;
 992
 993   const int next_line = scores->widthStep / sizeof(short);
 994   int h = scores->height;
 995   int w = scores->width;
 996
 997   CvRect roi = cvGetImageROI(image);
 998
 999   if (roi.x < int(R+1)) roi.x = R+1;
1000   if (roi.y < int(R+1)) roi.y = R+1;
1001   if ((roi.x + roi.width)  > int(scores->width-R-2))  roi.width  = scores->width  - R - roi.x - 2;
1002   if ((roi.y + roi.height) > int(scores->height-R-2)) roi.height = scores->height - R - roi.y - 2;
1003
1004   unsigned int xend = roi.x + roi.width;
1005   unsigned int yend = roi.y + roi.height;
1006
1007   for(unsigned int y = roi.y; y < yend; y++)
1008   {
1009     short * Scores = (short *)(scores->imageData + y * scores->widthStep);
1010
1011     for(unsigned int x = roi.x; x < xend; x++)
1012     {
1013       short * Sb = Scores + x;
1014
1015       // skip 0 score pixels
1016       if (abs(Sb[0]) < 5)
1017         ++x; // if this pixel is 0, the next one will not be good enough. Skip it.
1018       else
1019       {
1020         if (third_check(Sb, next_line) && is_local_maxima(Sb, R, scores))
1021         {
1022           keypoint p;
1023           p.u = float(x);
1024           p.v = float(y);
1025           p.scale = scale;
1026           p.score = float(abs(Sb[0]));
1027
1028           if (use_bins)
1029           {
1030             int bin_u_index = (bin_nb_u * x) / w;
1031             int bin_v_index = (bin_nb_v * y) / h;
1032
1033             if (bin_u_index >= bin_nb_u)
1034               bin_u_index = bin_nb_u - 1;
1035             if (bin_v_index >= bin_nb_v)
1036               bin_v_index = bin_nb_v - 1;
1037
1038             if (bins[bin_u_index][bin_v_index].size() < yape_bin_size)
1039               bins[bin_u_index][bin_v_index].push_back(p);
1040           }
1041           else
1042             tmp_points.push_back(p);
1043
1044           nbpts++;
1045           x += R-1;
1046         }
1047       }
1048     }
1049   }
1050   return nbpts;
1051 }
1052
1053 /////////////////////////////////////////////////////////////////
1054 // Sub-pixel / sub-scale accuracy.
1055 /////////////////////////////////////////////////////////////////
1056
1057 static float fit_quadratic_2x2(float x[2], float L[3][3])
1058 {
1059   float H[2][2];
1060   float b[2];
1061
1062   b[0] = -(L[1][2] - L[1][0]) / 2.0f;
1063   b[1] = -(L[2][1] - L[0][1]) / 2.0f;
1064
1065   H[0][0] = L[1][0] - 2.0f * L[1][1] + L[1][2];
1066   H[1][1] = L[0][1] - 2.0f * L[1][1] + L[2][1];
1067   H[0][1] = H[1][0] = (L[0][0] - L[0][2] - L[2][0] + L[2][2]) / 4.0f;
1068
1069   float t4 = 1/(H[0][0]*H[1][1]-H[0][1]*H[1][0]);
1070   x[0] =  H[1][1]*t4*b[0]-H[0][1]*t4*b[1];
1071   x[1] = -H[1][0]*t4*b[0]+H[0][0]*t4*b[1];
1072
1073   return 0.5f * (b[0] * x[0] + b[1] * x[1]);
1074 }
1075
1076 void yape::subpix_refine(IplImage *im, keypoint *p)
1077 {
1078   float L[3][3];
1079
1080   int px = (int) p->u;
1081   int py = (int) p->v;
1082   int dirs_nb = Dirs_nb[radius];
1083   short * dirs = Dirs->t[radius];
1084
1085   if ((px<= radius) || (px >= (scores->width-radius)) || (py <= radius) || (py >= (scores->height-radius)))
1086     return;
1087
1088   unsigned char * I = (unsigned char *) (im->imageData + py*im->widthStep) +  px;
1089   short * s = (short *) (scores->imageData + py*scores->widthStep) +  px;
1090   int line = scores->widthStep/sizeof(short);
1091
1092   for (int y=0; y<3; ++y) {
1093     int offset = (y-1)*line - 1;
1094     for (int x=0; x<3; ++x) {
1095       if (s[ offset + x]==0) {
1096         // Compute Laplacian
1097         int score = 0;
1098         for (int d=0; d<dirs_nb; ++d)
1099           score += I[offset + x + dirs[d]];
1100         L[y][x] = -float(score - dirs_nb*(int)I[offset + x]);
1101       } else {
1102         L[y][x] = (float)s[ offset + x];
1103       }
1104     }
1105   }
1106
1107   float delta[2];
1108   p->score += fit_quadratic_2x2(delta, L);
1109
1110   if ((delta[0] >= -1) && (delta[0] <= 1))
1111     p->u += delta[0];
1112   else
1113     p->u+=0.5f;
1114   if ((delta[1] >= -1) && (delta[1] <= 1))
1115     p->v += delta[1];
1116   else
1117     p->v+=0.5f;
1118 }
1119
1120 //////////////////////////////////////////////////////////////////////
1121 // PyrYape implementation
1122 //////////////////////////////////////////////////////////////////////
1123
1124 /*! \param w width passed to Yape constructor
1125 \param h height passed to Yape constructor
1126 \param nbLev pyramid depth
1127 */
1128 pyr_yape::pyr_yape(int w, int h, int nbLev)
1129 : yape (w,h)
1130 {
1131   internal_pim = 0;
1132   pscores = new PyrImage(scores, nbLev);
1133   pDirs[0] = Dirs;
1134   pDirs_nb[0] = Dirs_nb;
1135   equalize = false;
1136
1137   PyrImage pim(cvCreateImage(cvSize(w, h), IPL_DEPTH_8U, 1),nbLev);
1138
1139   for (int i=1; i<nbLev; i++) {
1140     pDirs[i] = new dir_table;
1141     pDirs_nb[i] = new int[yape_max_radius];
1142     for(int R = 1; R < yape_max_radius; R++)
1143       precompute_directions(pim[i], pDirs[i]->t[R], &(pDirs_nb[i][R]), R);
1144   }
1145 }
1146
1147 pyr_yape::~pyr_yape()
1148 {
1149   if (internal_pim) delete internal_pim;
1150
1151   for (int i=0; i<pscores->nbLev; i++) {
1152     delete pDirs[i];
1153     delete[] pDirs_nb[i];
1154   }
1155   Dirs=0;
1156   Dirs_nb=0;
1157   delete pscores;
1158   pscores = 0;
1159   scores = 0;
1160 }
1161
1162 void pyr_yape::select_level(int l)
1163 {
1164   scores = pscores->images[l];
1165   Dirs = pDirs[l];
1166   Dirs_nb = pDirs_nb[l];
1167 }
1168
1169 /*! Detect features on the pyramid, filling the scale field of keypoints with
1170 * the pyramid level. \return the detected keypoint number.
1171 */
1172 int pyr_yape::detect(PyrImage *image, keypoint *points, int max_point_number)
1173 {
1174   reserve_tmp_arrays();
1175
1176     PROFILE_SECTION_PUSH("detect + maxima" );
1177   for (int i=image->nbLev-1; i>=0; --i)
1178   {
1179     select_level(i);
1180     raw_detect_mt(image->images[i]);
1181     get_local_maxima(image->images[i], radius, float(i));
1182   }
1183   PROFILE_SECTION_POP();
1184
1185   PROFILE_SECTION_PUSH("pick best, refine");
1186   int n = pick_best_points(points, max_point_number);
1187   for (int i = 0; i < n; i++) {
1188     int l =(int)points[i].scale;
1189     select_level(l);
1190     subpix_refine(image->images[l], points + i);
1191   }
1192   PROFILE_SECTION_POP();
1193
1194   return n;
1195 }
1196
1197 /*!
1198 * This method does the following:
1199 * 1) Apply a Gaussian blur filter on the provided image, putting the result in
1200 *    the pyramid lowest level.
1201 * 2) Build the pyramid
1202 * 3) call detect() on it.
1203 *
1204 * If no pyramid is given by the caller, a temporary pyramid image is created
1205 * and recycled for future calls.
1206 *
1207 * \param im the input image. Must be 1 channel (gray levels).
1208 * \param points an array to store keypoints.
1209 * \param max_point_number the size of this array.
1210 * \param caller_pim the pyramid to work on, or 0 for an internal pyramid.
1211 */
1212 int pyr_yape::pyramidBlurDetect(IplImage *im, keypoint *points, int max_point_number, PyrImage *caller_pim)
1213 {
1214     PROFILE_THIS_FUNCTION();
1215   assert(im->nChannels == 1);
1216
1217   PyrImage *pim;
1218
1219   if (caller_pim == 0)
1220   {
1221       PROFILE_THIS_BLOCK("create pim");
1222     if (internal_pim && ((internal_pim->images[0]->width != im->width)
1223         || (internal_pim->images[0]->height != im->height)))
1224     {
1225       delete internal_pim;
1226       internal_pim = 0;
1227     }
1228
1229     if (internal_pim == 0)
1230       internal_pim = new PyrImage(cvCreateImage(cvGetSize(im), IPL_DEPTH_8U, 1), pscores->nbLev);
1231
1232     pim = internal_pim;
1233   }
1234   else
1235   {
1236     pim = caller_pim;
1237     assert (im->width == caller_pim->images[0]->width);
1238   }
1239
1240   int gaussian_filter_size = 3;
1241   if (radius >= 5) gaussian_filter_size = 5;
1242   if (radius >= 7) gaussian_filter_size = 7;
1243
1244   PROFILE_SECTION_PUSH("gaussian");
1245   cvSmooth(im, pim->images[0], CV_GAUSSIAN, gaussian_filter_size, gaussian_filter_size);
1246   PROFILE_SECTION_POP();
1247
1248   PROFILE_SECTION_PUSH("build");
1249   pim->build();
1250   PROFILE_SECTION_POP();
1251
1252   PROFILE_SECTION_PUSH("detect");
1253   int res = detect(pim, points, max_point_number);
1254   PROFILE_SECTION_POP();
1255   return res;
1256 }
1257
1258 void pyr_yape::save_image_of_detected_points(char * name, IplImage * image, keypoint * points, int points_nb)
1259 {
1260   IplImage * point_image = mcvGrayToColor(image);
1261   for(int i = 0; i < points_nb; i++)
1262     mcvCircle(point_image,
1263               (int)PyrImage::convCoordf(points[i].u, (int)points[i].scale, 0),
1264               (int)PyrImage::convCoordf(points[i].v, (int)points[i].scale, 0),
1265               PyrImage::convCoord(2 * radius, (int)points[i].scale, 0), mcvRainbowColor((int)points[i].scale), 1);
1266   mcvSaveImage(name, point_image);
1267   cvReleaseImage(&point_image);
1268 }
1269
1270 void pyr_yape::stat_points(keypoint *points, int nb_pts)
1271 {
1272   int *histogram = new int[pscores->nbLev];
1273
1274   for (int l=0; l< pscores->nbLev; ++l) histogram[l]=0;
1275
1276   for (int i=0; i<nb_pts; ++i)
1277     histogram[(int)(points[i].scale)]++;
1278
1279   for (int l=0; l< pscores->nbLev; ++l)
1280     cout << "Level " << l << ": " << histogram[l] << " keypoints ("
1281          << 100.0f * (float)histogram[l]/(float)nb_pts << "%)\n";
1282
1283   delete[] histogram;
1284 }
1285