vcs.maemo.org Git - opencv/blob - src/cv/cvstardetector.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2008, Willow Garage Inc., all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cv.h"
  43
  44 static void
  45 icvComputeIntegralImages( const CvMat* _I, CvMat* _S, CvMat* _T, CvMat* _FT )
  46 {
  47     int x, y, rows = _I->rows, cols = _I->cols;
  48     const uchar* I = _I->data.ptr;
  49     int *S = _S->data.i, *T = _T->data.i, *FT = _FT->data.i;
  50     int istep = _I->step, step = _S->step/sizeof(S[0]);
  51
  52     assert( CV_MAT_TYPE(_I->type) == CV_8UC1 &&
  53         CV_MAT_TYPE(_S->type) == CV_32SC1 &&
  54         CV_ARE_TYPES_EQ(_S, _T) && CV_ARE_TYPES_EQ(_S, _FT) &&
  55         CV_ARE_SIZES_EQ(_S, _T) && CV_ARE_SIZES_EQ(_S, _FT) &&
  56         _S->step == _T->step && _S->step == _FT->step &&
  57         _I->rows+1 == _S->rows && _I->cols+1 == _S->cols );
  58
  59     for( x = 0; x <= cols; x++ )
  60         S[x] = T[x] = FT[x] = 0;
  61
  62     S += step; T += step; FT += step;
  63     S[0] = T[0] = 0;
  64     FT[0] = I[0];
  65     for( x = 1; x < cols; x++ )
  66     {
  67         S[x] = S[x-1] + I[x-1];
  68         T[x] = I[x-1];
  69         FT[x] = I[x] + I[x-1];
  70     }
  71     S[cols] = S[cols-1] + I[cols-1];
  72     T[cols] = FT[cols] = I[cols-1];
  73
  74     for( y = 2; y <= rows; y++ )
  75     {
  76         I += istep, S += step, T += step, FT += step;
  77
  78         S[0] = S[-step]; S[1] = S[-step+1] + I[0];
  79         T[0] = T[-step + 1];
  80         T[1] = FT[0] = T[-step + 2] + I[-istep] + I[0];
  81         FT[1] = FT[-step + 2] + I[-istep] + I[1] + I[0];
  82
  83         for( x = 2; x < cols; x++ )
  84         {
  85             S[x] = S[x - 1] + S[-step + x] - S[-step + x - 1] + I[x - 1];
  86             T[x] = T[-step + x - 1] + T[-step + x + 1] - T[-step*2 + x] + I[-istep + x - 1] + I[x - 1];
  87             FT[x] = FT[-step + x - 1] + FT[-step + x + 1] - FT[-step*2 + x] + I[x] + I[x-1];
  88         }
  89
  90         S[cols] = S[cols - 1] + S[-step + cols] - S[-step + cols - 1] + I[cols - 1];
  91         T[cols] = FT[cols] = T[-step + cols - 1] + I[-istep + cols - 1] + I[cols - 1];
  92     }
  93 }
  94
  95 typedef struct CvStarFeature
  96 {
  97     int area;
  98     int* p[8];
  99 }
 100 CvStarFeature;
 101
 102 static int
 103 icvStarDetectorComputeResponses( const CvMat* img, CvMat* responses, CvMat* sizes,
 104                                  const CvStarDetectorParams* params )
 105 {
 106     const int MAX_PATTERN = 17;
 107     static const int sizes0[] = {1, 2, 3, 4, 6, 8, 11, 12, 16, 22, 23, 32, 45, 46, 64, 90, 128, -1};
 108     static const int pairs[][2] = {{1, 0}, {3, 1}, {4, 2}, {5, 3}, {7, 4}, {8, 5}, {9, 6},
 109                                    {11, 8}, {13, 10}, {14, 11}, {15, 12}, {16, 14}, {-1, -1}};
 110     float invSizes[MAX_PATTERN][2];
 111     int sizes1[MAX_PATTERN];
 112
 113 #if CV_SSE2
 114     __m128 invSizes4[MAX_PATTERN][2];
 115     __m128 sizes1_4[MAX_PATTERN];
 116     Cv32suf absmask;
 117     absmask.i = 0x7fffffff;
 118     __m128 absmask4 = _mm_set1_ps(absmask.f);
 119 #endif
 120     CvStarFeature f[MAX_PATTERN];
 121
 122     CvMat *sum = 0, *tilted = 0, *flatTilted = 0;
 123     int x, y, i=0, rows = img->rows, cols = img->cols, step;
 124     int border, npatterns=0, maxIdx=0;
 125 #ifdef _OPENMP
 126     int nthreads = cvGetNumThreads();
 127 #endif
 128
 129     assert( CV_MAT_TYPE(img->type) == CV_8UC1 &&
 130         CV_MAT_TYPE(responses->type) == CV_32FC1 &&
 131         CV_MAT_TYPE(sizes->type) == CV_16SC1 &&
 132         CV_ARE_SIZES_EQ(responses, sizes) );
 133
 134     for(; pairs[i][0] >= 0; i++ )
 135     {
 136         if( sizes0[pairs[i][0]] >= params->maxSize )
 137             break;
 138     }
 139     npatterns = i;
 140     npatterns += (pairs[npatterns-1][0] >= 0);
 141     maxIdx = pairs[npatterns-1][0];
 142
 143     sum = cvCreateMat( rows + 1, cols + 1, CV_32SC1 );
 144     tilted = cvCreateMat( rows + 1, cols + 1, CV_32SC1 );
 145     flatTilted = cvCreateMat( rows + 1, cols + 1, CV_32SC1 );
 146     step = sum->step/CV_ELEM_SIZE(sum->type);
 147
 148     icvComputeIntegralImages( img, sum, tilted, flatTilted );
 149
 150     for( i = 0; i <= maxIdx; i++ )
 151     {
 152         int ur_size = sizes0[i], t_size = sizes0[i] + sizes0[i]/2;
 153         int ur_area = (2*ur_size + 1)*(2*ur_size + 1);
 154         int t_area = t_size*t_size + (t_size + 1)*(t_size + 1);
 155
 156         f[i].p[0] = sum->data.i + (ur_size + 1)*step + ur_size + 1;
 157         f[i].p[1] = sum->data.i - ur_size*step + ur_size + 1;
 158         f[i].p[2] = sum->data.i + (ur_size + 1)*step - ur_size;
 159         f[i].p[3] = sum->data.i - ur_size*step - ur_size;
 160
 161         f[i].p[4] = tilted->data.i + (t_size + 1)*step + 1;
 162         f[i].p[5] = flatTilted->data.i - t_size;
 163         f[i].p[6] = flatTilted->data.i + t_size + 1;
 164         f[i].p[7] = tilted->data.i - t_size*step + 1;
 165
 166         f[i].area = ur_area + t_area;
 167         sizes1[i] = sizes0[i];
 168     }
 169     // negate end points of the size range
 170     // for a faster rejection of very small or very large features in non-maxima suppression.
 171     sizes1[0] = -sizes1[0];
 172     sizes1[1] = -sizes1[1];
 173     sizes1[maxIdx] = -sizes1[maxIdx];
 174     border = sizes0[maxIdx] + sizes0[maxIdx]/2;
 175
 176     for( i = 0; i < npatterns; i++ )
 177     {
 178         int innerArea = f[pairs[i][1]].area;
 179         int outerArea = f[pairs[i][0]].area - innerArea;
 180         invSizes[i][0] = 1.f/outerArea;
 181         invSizes[i][1] = 1.f/innerArea;
 182 #if CV_SSE2
 183         _mm_store_ps((float*)&invSizes4[i][0], _mm_set1_ps(invSizes[i][0]));
 184         _mm_store_ps((float*)&invSizes4[i][1], _mm_set1_ps(invSizes[i][1]));
 185     }
 186
 187     for( i = 0; i <= maxIdx; i++ )
 188         _mm_store_ps((float*)&sizes1_4[i], _mm_set1_ps((float)sizes1[i]));
 189 #else
 190     }
 191 #endif
 192
 193     for( y = 0; y < border; y++ )
 194     {
 195         float* r_ptr = (float*)(responses->data.ptr + responses->step*y);
 196         float* r_ptr2 = (float*)(responses->data.ptr + responses->step*(rows - 1 - y));
 197         short* s_ptr = (short*)(sizes->data.ptr + sizes->step*y);
 198         short* s_ptr2 = (short*)(sizes->data.ptr + sizes->step*(rows - 1 - y));
 199         for( x = 0; x < cols; x++ )
 200         {
 201             r_ptr[x] = r_ptr2[x] = 0;
 202             s_ptr[x] = s_ptr2[x] = 0;
 203         }
 204     }
 205
 206 #ifdef _OPENMP
 207     #pragma omp parallel for num_threads(nthreads) schedule(static)
 208 #endif
 209     for( y = border; y < rows - border; y++ )
 210     {
 211         int x, i;
 212         float* r_ptr = (float*)(responses->data.ptr + responses->step*y);
 213         short* s_ptr = (short*)(sizes->data.ptr + sizes->step*y);
 214         for( x = 0; x < border; x++ )
 215         {
 216             r_ptr[x] = r_ptr[cols - 1 - x] = 0;
 217             s_ptr[x] = s_ptr[cols - 1 - x] = 0;
 218         }
 219
 220 #if CV_SSE2
 221         for( ; x <= cols - border - 4; x += 4 )
 222         {
 223             int ofs = y*step + x;
 224             __m128 vals[MAX_PATTERN];
 225             __m128 bestResponse = _mm_setzero_ps();
 226             __m128 bestSize = _mm_setzero_ps();
 227
 228             for( i = 0; i <= maxIdx; i++ )
 229             {
 230                 const int** p = (const int**)&f[i].p[0];
 231                 __m128i r0 = _mm_sub_epi32(_mm_loadu_si128((const __m128i*)(p[0]+ofs)),
 232                                            _mm_loadu_si128((const __m128i*)(p[1]+ofs)));
 233                 __m128i r1 = _mm_sub_epi32(_mm_loadu_si128((const __m128i*)(p[3]+ofs)),
 234                                            _mm_loadu_si128((const __m128i*)(p[2]+ofs)));
 235                 __m128i r2 = _mm_sub_epi32(_mm_loadu_si128((const __m128i*)(p[4]+ofs)),
 236                                            _mm_loadu_si128((const __m128i*)(p[5]+ofs)));
 237                 __m128i r3 = _mm_sub_epi32(_mm_loadu_si128((const __m128i*)(p[7]+ofs)),
 238                                            _mm_loadu_si128((const __m128i*)(p[6]+ofs)));
 239                 r0 = _mm_add_epi32(_mm_add_epi32(r0,r1), _mm_add_epi32(r2,r3));
 240                 _mm_store_ps((float*)&vals[i], _mm_cvtepi32_ps(r0));
 241             }
 242
 243             for( i = 0; i < npatterns; i++ )
 244             {
 245                 __m128 inner_sum = vals[pairs[i][1]];
 246                 __m128 outer_sum = _mm_sub_ps(vals[pairs[i][0]], inner_sum);
 247                 __m128 response = _mm_sub_ps(_mm_mul_ps(inner_sum, invSizes4[i][1]),
 248                     _mm_mul_ps(outer_sum, invSizes4[i][0]));
 249                 __m128 swapmask = _mm_cmpgt_ps(_mm_and_ps(response,absmask4),
 250                     _mm_and_ps(bestResponse,absmask4));
 251                 bestResponse = _mm_xor_ps(bestResponse,
 252                     _mm_and_ps(_mm_xor_ps(response,bestResponse), swapmask));
 253                 bestSize = _mm_xor_ps(bestSize,
 254                     _mm_and_ps(_mm_xor_ps(sizes1_4[pairs[i][0]], bestSize), swapmask));
 255             }
 256
 257             _mm_storeu_ps(r_ptr + x, bestResponse);
 258             _mm_storel_epi64((__m128i*)(s_ptr + x),
 259                 _mm_packs_epi32(_mm_cvtps_epi32(bestSize),_mm_setzero_si128()));
 260         }
 261 #endif
 262         for( ; x < cols - border; x++ )
 263         {
 264             int ofs = y*step + x;
 265             int vals[MAX_PATTERN];
 266             float bestResponse = 0;
 267             int bestSize = 0;
 268
 269             for( i = 0; i <= maxIdx; i++ )
 270             {
 271                 const int** p = (const int**)&f[i].p[0];
 272                 vals[i] = p[0][ofs] - p[1][ofs] - p[2][ofs] + p[3][ofs] +
 273                     p[4][ofs] - p[5][ofs] - p[6][ofs] + p[7][ofs];
 274             }
 275             for( i = 0; i < npatterns; i++ )
 276             {
 277                 int inner_sum = vals[pairs[i][1]];
 278                 int outer_sum = vals[pairs[i][0]] - inner_sum;
 279                 float response = inner_sum*invSizes[i][1] - outer_sum*invSizes[i][0];
 280                 if( fabs(response) > fabs(bestResponse) )
 281                 {
 282                     bestResponse = response;
 283                     bestSize = sizes1[pairs[i][0]];
 284                 }
 285             }
 286
 287             r_ptr[x] = bestResponse;
 288             s_ptr[x] = (short)bestSize;
 289         }
 290     }
 291
 292     cvReleaseMat(&sum);
 293     cvReleaseMat(&tilted);
 294     cvReleaseMat(&flatTilted);
 295
 296     return border;
 297 }
 298
 299
 300 static bool
 301 icvStarDetectorSuppressLines( const CvMat* responses, const CvMat* sizes, CvPoint pt,
 302                               const CvStarDetectorParams* params )
 303 {
 304     const float* r_ptr = responses->data.fl;
 305     int rstep = responses->step/sizeof(r_ptr[0]);
 306     const short* s_ptr = sizes->data.s;
 307     int sstep = sizes->step/sizeof(s_ptr[0]);
 308     int sz = s_ptr[pt.y*sstep + pt.x];
 309     int x, y, delta = sz/4, radius = delta*4;
 310     float Lxx = 0, Lyy = 0, Lxy = 0;
 311     int Lxxb = 0, Lyyb = 0, Lxyb = 0;
 312
 313     for( y = pt.y - radius; y <= pt.y + radius; y += delta )
 314         for( x = pt.x - radius; x <= pt.x + radius; x += delta )
 315         {
 316             float Lx = r_ptr[y*rstep + x + 1] - r_ptr[y*rstep + x - 1];
 317             float Ly = r_ptr[(y+1)*rstep + x] - r_ptr[(y-1)*rstep + x];
 318             Lxx += Lx*Lx; Lyy += Ly*Ly; Lxy += Lx*Ly;
 319         }
 320
 321     if( (Lxx + Lyy)*(Lxx + Lyy) >= params->lineThresholdProjected*(Lxx*Lyy - Lxy*Lxy) )
 322         return true;
 323
 324     for( y = pt.y - radius; y <= pt.y + radius; y += delta )
 325         for( x = pt.x - radius; x <= pt.x + radius; x += delta )
 326         {
 327             int Lxb = (s_ptr[y*sstep + x + 1] == sz) - (s_ptr[y*sstep + x - 1] == sz);
 328             int Lyb = (s_ptr[(y+1)*sstep + x] == sz) - (s_ptr[(y-1)*sstep + x] == sz);
 329             Lxxb += Lxb * Lxb; Lyyb += Lyb * Lyb; Lxyb += Lxb * Lyb;
 330         }
 331
 332     if( (Lxxb + Lyyb)*(Lxxb + Lyyb) >= params->lineThresholdBinarized*(Lxxb*Lyyb - Lxyb*Lxyb) )
 333         return true;
 334
 335     return false;
 336 }
 337
 338
 339 static void
 340 icvStarDetectorSuppressNonmax( const CvMat* responses, const CvMat* sizes,
 341                                CvSeq* keypoints, int border,
 342                                const CvStarDetectorParams* params )
 343 {
 344     int x, y, x1, y1, delta = params->suppressNonmaxSize/2;
 345     int rows = responses->rows, cols = responses->cols;
 346     const float* r_ptr = responses->data.fl;
 347     int rstep = responses->step/sizeof(r_ptr[0]);
 348     const short* s_ptr = sizes->data.s;
 349     int sstep = sizes->step/sizeof(s_ptr[0]);
 350     short featureSize = 0;
 351
 352     for( y = border; y < rows - border; y += delta+1 )
 353         for( x = border; x < cols - border; x += delta+1 )
 354         {
 355             float maxResponse = (float)params->responseThreshold;
 356             float minResponse = (float)-params->responseThreshold;
 357             CvPoint maxPt = {-1,-1}, minPt = {-1,-1};
 358             int tileEndY = MIN(y + delta, rows - border - 1);
 359             int tileEndX = MIN(x + delta, cols - border - 1);
 360
 361             for( y1 = y; y1 <= tileEndY; y1++ )
 362                 for( x1 = x; x1 <= tileEndX; x1++ )
 363                 {
 364                     float val = r_ptr[y1*rstep + x1];
 365                     if( maxResponse < val )
 366                     {
 367                         maxResponse = val;
 368                         maxPt = cvPoint(x1, y1);
 369                     }
 370                     else if( minResponse > val )
 371                     {
 372                         minResponse = val;
 373                         minPt = cvPoint(x1, y1);
 374                     }
 375                 }
 376
 377             if( maxPt.x >= 0 )
 378             {
 379                 for( y1 = maxPt.y - delta; y1 <= maxPt.y + delta; y1++ )
 380                     for( x1 = maxPt.x - delta; x1 <= maxPt.x + delta; x1++ )
 381                     {
 382                         float val = r_ptr[y1*rstep + x1];
 383                         if( val >= maxResponse && (y1 != maxPt.y || x1 != maxPt.x))
 384                             goto skip_max;
 385                     }
 386
 387                 if( (featureSize = s_ptr[maxPt.y*sstep + maxPt.x]) >= 4 &&
 388                     !icvStarDetectorSuppressLines( responses, sizes, maxPt, params ))
 389                 {
 390                     CvStarKeypoint kpt = cvStarKeypoint( maxPt, featureSize, maxResponse );
 391                     cvSeqPush( keypoints, &kpt );
 392                 }
 393             }
 394         skip_max:
 395             if( minPt.x >= 0 )
 396             {
 397                 for( y1 = minPt.y - delta; y1 <= minPt.y + delta; y1++ )
 398                     for( x1 = minPt.x - delta; x1 <= minPt.x + delta; x1++ )
 399                     {
 400                         float val = r_ptr[y1*rstep + x1];
 401                         if( val <= minResponse && (y1 != minPt.y || x1 != minPt.x))
 402                             goto skip_min;
 403                     }
 404
 405                 if( (featureSize = s_ptr[minPt.y*sstep + minPt.x]) >= 4 &&
 406                     !icvStarDetectorSuppressLines( responses, sizes, minPt, params ))
 407                 {
 408                     CvStarKeypoint kpt = cvStarKeypoint( minPt, featureSize, minResponse );
 409                     cvSeqPush( keypoints, &kpt );
 410                 }
 411             }
 412         skip_min:
 413             ;
 414         }
 415 }
 416
 417 CV_IMPL CvSeq*
 418 cvGetStarKeypoints( const CvArr* _img, CvMemStorage* storage,
 419                     CvStarDetectorParams params )
 420 {
 421     CvMat stub, *img = cvGetMat(_img, &stub);
 422     CvSeq* keypoints = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvStarKeypoint), storage );
 423     CvMat* responses = cvCreateMat( img->rows, img->cols, CV_32FC1 );
 424     CvMat* sizes = cvCreateMat( img->rows, img->cols, CV_16SC1 );
 425
 426     int border = icvStarDetectorComputeResponses( img, responses, sizes, &params );
 427     if( border >= 0 )
 428         icvStarDetectorSuppressNonmax( responses, sizes, keypoints, border, &params );
 429
 430     cvReleaseMat( &responses );
 431     cvReleaseMat( &sizes );
 432
 433     return border >= 0 ? keypoints : 0;
 434 }
 435
 436 namespace cv
 437 {
 438
 439 StarDetector::StarDetector()
 440 {
 441     *(CvStarDetectorParams*)this = cvStarDetectorParams();
 442 }
 443
 444 StarDetector::StarDetector(int _maxSize, int _responseThreshold,
 445                            int _lineThresholdProjected,
 446                            int _lineThresholdBinarized,
 447                            int _suppressNonmaxSize)
 448 {
 449     *(CvStarDetectorParams*)this = cvStarDetectorParams(_maxSize, _responseThreshold,
 450             _lineThresholdProjected, _lineThresholdBinarized, _suppressNonmaxSize);
 451 }
 452
 453 void StarDetector::operator()(const Mat& image, vector<KeyPoint>& keypoints) const
 454 {
 455     CvMat _image = image;
 456     MemStorage storage(cvCreateMemStorage(0));
 457     Seq<CvStarKeypoint> kp = cvGetStarKeypoints( &_image, storage, *(const CvStarDetectorParams*)this);
 458     Seq<CvStarKeypoint>::iterator it = kp.begin();
 459     keypoints.resize(kp.size());
 460     size_t i, n = kp.size();
 461     for( i = 0; i < n; i++, ++it )
 462     {
 463         const CvStarKeypoint& kpt = *it;
 464         keypoints[i] = KeyPoint(kpt.pt, (float)kpt.size, -1.f, kpt.response, 0);
 465     }
 466 }
 467
 468 }