git.maemo.org Git - opencv/blob - cv/src/cvpyrsegmentation.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 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, 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 #include "_cv.h"
  42
  43 typedef struct _CvRGBf
  44 {   float blue;
  45     float green;
  46     float red;
  47 }
  48 _CvRGBf;
  49
  50 typedef struct _CvRect16u
  51 {
  52     ushort x1, y1, x2, y2;
  53 }
  54 _CvRect16u;
  55
  56 typedef struct _CvPyramid
  57 {
  58     float c;
  59     struct _CvPyramid *p;
  60     int a;
  61     _CvRect16u rect;      /*  ROI for the connected component    */
  62 } _CvPyramid;
  63
  64 /* element of base layer */
  65 typedef struct _CvPyramidBase
  66 {
  67     float c;
  68     struct _CvPyramid *p;
  69 }
  70 _CvPyramidBase;
  71
  72 typedef struct _CvPyramidC3
  73 {
  74     _CvRGBf c;
  75     struct _CvPyramidC3 *p;
  76     int a;
  77     _CvRect16u rect;      /*  ROI for the connected component    */
  78 } _CvPyramidC3;
  79
  80 /* element of base layer */
  81 typedef struct _CvPyramidBaseC3
  82 {
  83     _CvRGBf c;
  84     struct _CvPyramidC3 *p;
  85 }
  86 _CvPyramidBaseC3;
  87
  88 typedef struct _CvListNode
  89 {
  90     struct _CvListNode* next;
  91     void* data;
  92 }
  93 _CvListNode;
  94
  95
  96 static CvStatus  icvSegmentClusterC1( CvSeq* cmp_seq, CvSeq* res_seq,
  97                                  double threshold,
  98                                  _CvPyramid* first_level_end,
  99                                  CvSize first_level_size );
 100
 101 static CvStatus  icvSegmentClusterC3( CvSeq* cmp_seq, CvSeq* res_seq,
 102                                  double threshold,
 103                                  _CvPyramidC3* first_level_end,
 104                                  CvSize first_level_size );
 105
 106 static CvStatus icvUpdatePyrLinks_8u_C1
 107     (int layer, void *layer_data, CvSize size, void *parent_layer,
 108      void *_writer, float threshold, int is_last_iter, void *_stub, CvWriteNodeFunction /*func*/);
 109
 110 static CvStatus icvUpdatePyrLinks_8u_C3
 111     (int layer, void *layer_data, CvSize size, void *parent_layer,
 112      void *_writer, float threshold, int is_last_iter, void *_stub, CvWriteNodeFunction /*func*/);
 113
 114 static void icvMaxRoi( _CvRect16u *max_rect, _CvRect16u* cur_rect );
 115 static void icvMaxRoi1( _CvRect16u *max_rect, int x, int y );
 116
 117
 118 #define _CV_CHECK( icvFun )                                             \
 119   {                                                                     \
 120     if( icvFun != CV_OK )                                               \
 121      goto M_END;                                                        \
 122   }
 123
 124
 125 #define _CV_MAX3( a, b, c) ((a)>(b) ? ((a)>(c) ? (a) : (c)) : ((b)>(c) ? (b) : (c)))
 126
 127 /*#define _CV_RGB_DIST(a, b)  _CV_MAX3((float)fabs((a).red - (b).red),      \
 128                                        (float)fabs((a).green - (b).green),  \
 129                                        (float)fabs((a).blue - (b).blue))*/
 130
 131 #define _CV_NEXT_BASE_C1(p,n) (_CvPyramid*)((char*)(p) + (n)*sizeof(_CvPyramidBase))
 132 #define _CV_NEXT_BASE_C3(p,n) (_CvPyramidC3*)((char*)(p) + (n)*sizeof(_CvPyramidBaseC3))
 133
 134
 135 CV_INLINE float icvRGBDist_Max( const _CvRGBf& a, const _CvRGBf& b )
 136 {
 137     float tr = (float)fabs(a.red - b.red);
 138     float tg = (float)fabs(a.green - b.green);
 139     float tb = (float)fabs(a.blue - b.blue);
 140
 141     return _CV_MAX3( tr, tg, tb );
 142 }
 143
 144 CV_INLINE float icvRGBDist_Sum( const _CvRGBf& a, const _CvRGBf& b )
 145 {
 146     float tr = (float)fabs(a.red - b.red);
 147     float tg = (float)fabs(a.green - b.green);
 148     float tb = (float)fabs(a.blue - b.blue);
 149
 150     return (tr + tg + tb);
 151 }
 152
 153 #if 1
 154 #define _CV_RGB_DIST  icvRGBDist_Max
 155 #define _CV_RGB_THRESH_SCALE   1
 156 #else
 157 #define _CV_RGB_DIST  icvRGBDist_Sum
 158 #define _CV_RGB_THRESH_SCALE   3
 159 #endif
 160
 161 #define _CV_INV_TAB_SIZE   32
 162
 163 static const float icvInvTab[ /*_CV_INV_TAB_SIZE*/ ] =
 164 {
 165     1.00000000f, 0.50000000f, 0.33333333f, 0.25000000f, 0.20000000f, 0.16666667f,
 166     0.14285714f, 0.12500000f, 0.11111111f, 0.10000000f, 0.09090909f, 0.08333333f,
 167     0.07692308f, 0.07142857f, 0.06666667f, 0.06250000f, 0.05882353f, 0.05555556f,
 168     0.05263158f, 0.05000000f, 0.04761905f, 0.04545455f, 0.04347826f, 0.04166667f,
 169     0.04000000f, 0.03846154f, 0.03703704f, 0.03571429f, 0.03448276f, 0.03333333f,
 170     0.03225806f, 0.03125000f
 171 };
 172
 173 static void
 174 icvWritePyrNode( void *elem, void *writer )
 175 {
 176     CV_WRITE_SEQ_ELEM( *(_CvListNode *) elem, *(CvSeqWriter *) writer );
 177 }
 178
 179
 180 static CvStatus
 181 icvPyrSegmentation8uC1R( uchar * src_image, int src_step,
 182                          uchar * dst_image, int dst_step,
 183                          CvSize roi, CvFilter filter,
 184                          CvSeq ** dst_comp, CvMemStorage * storage,
 185                          int level, int threshold1, int threshold2 )
 186 {
 187     int i, j, l;
 188     int step;
 189     const int max_iter = 3;     /* maximum number of iterations */
 190     int cur_iter = 0;           /* current iteration */
 191
 192     _CvPyramid *pyram[16];      /* pointers to the pyramid down up to level */
 193
 194     float *pyramida = 0;
 195     _CvPyramid stub;
 196
 197     _CvPyramid *p_cur;
 198     _CvPyramidBase *p_base;
 199     _CvListNode cmp_node;
 200
 201     CvSeq *cmp_seq = 0;
 202     CvSeq *res_seq = 0;
 203     CvMemStorage *temp_storage = 0;
 204     CvSize size;
 205     CvStatus status;
 206     CvSeqWriter writer;
 207
 208     int buffer_size;
 209     char *buffer = 0;
 210
 211     status = CV_OK;
 212
 213     /* clear pointer to resultant sequence */
 214     if( dst_comp )
 215         *dst_comp = 0;
 216
 217     /* check args */
 218     if( !src_image || !dst_image || !storage || !dst_comp )
 219         return CV_NULLPTR_ERR;
 220     if( roi.width <= 0 || roi.height <= 0 || src_step < roi.width || dst_step < roi.width )
 221         return CV_BADSIZE_ERR;
 222     if( filter != CV_GAUSSIAN_5x5 )
 223         return CV_BADRANGE_ERR;
 224     if( threshold1 < 0 || threshold2 < 0 )
 225         return CV_BADRANGE_ERR;
 226     if( level <= 0 )
 227         return CV_BADRANGE_ERR;
 228
 229     if( ((roi.width | roi.height) & ((1 << level) - 1)) != 0 )
 230         return CV_BADCOEF_ERR;
 231
 232     temp_storage = cvCreateChildMemStorage( storage );
 233
 234     /* sequence for temporary components */
 235     cmp_seq = cvCreateSeq( 0, sizeof( CvSeq ), sizeof( _CvListNode ), temp_storage );
 236     assert( cmp_seq != 0 );
 237
 238     res_seq = cvCreateSeq( CV_SEQ_CONNECTED_COMP, sizeof( CvSeq ),
 239                            sizeof( CvConnectedComp ), storage );
 240     assert( res_seq != 0 );
 241
 242     /* calculate buffer size */
 243     buffer_size = roi.width * roi.height * (sizeof( float ) + sizeof( _CvPyramidBase ));
 244
 245     for( l = 1; l <= level; l++ )
 246         buffer_size += ((roi.width >> l) + 1) * ((roi.height >> l) + 1) * sizeof(_CvPyramid);
 247
 248     /* allocate buffer */
 249     buffer = (char *) cvAlloc( buffer_size );
 250     if( !buffer )
 251     {
 252         status = CV_OUTOFMEM_ERR;
 253         goto M_END;
 254     }
 255
 256     pyramida = (float *) buffer;
 257
 258     /* initialization pyramid-linking properties down up to level */
 259     step = roi.width * sizeof( float );
 260
 261     {
 262         CvMat _src;
 263         CvMat _pyramida;
 264         cvInitMatHeader( &_src, roi.height, roi.width, CV_8UC1, src_image, src_step );
 265         cvInitMatHeader( &_pyramida, roi.height, roi.width, CV_32FC1, pyramida, step );
 266         cvConvert( &_src, &_pyramida );
 267         /*_CV_CHECK( icvCvtTo_32f_C1R( src_image, src_step, pyramida, step, roi, CV_8UC1 ));*/
 268     }
 269     p_base = (_CvPyramidBase *) (buffer + step * roi.height);
 270     pyram[0] = (_CvPyramid *) p_base;
 271
 272     /* fill base level of pyramid */
 273     for( i = 0; i < roi.height; i++ )
 274     {
 275         for( j = 0; j < roi.width; j++, p_base++ )
 276         {
 277             p_base->c = pyramida[i * roi.width + j];
 278             p_base->p = &stub;
 279         }
 280     }
 281
 282     p_cur = (_CvPyramid *) p_base;
 283     size = roi;
 284
 285     /* calculate initial pyramid */
 286     for( l = 1; l <= level; l++ )
 287     {
 288         CvSize dst_size = { size.width/2+1, size.height/2+1 };
 289         CvMat prev_level = cvMat( size.height, size.width, CV_32FC1 );
 290         CvMat next_level = cvMat( dst_size.height, dst_size.width, CV_32FC1 );
 291
 292         cvSetData( &prev_level, pyramida, step );
 293         cvSetData( &next_level, pyramida, step );
 294         cvPyrDown( &prev_level, &next_level );
 295
 296         //_CV_CHECK( icvPyrDown_Gauss5x5_32f_C1R( pyramida, step, pyramida, step, size, buff ));
 297         //_CV_CHECK( icvPyrDownBorder_32f_CnR( pyramida, step, size, pyramida, step, dst_size, 1 ));
 298         pyram[l] = p_cur;
 299
 300         size.width = dst_size.width - 1;
 301         size.height = dst_size.height - 1;
 302
 303         /* fill layer #l */
 304         for( i = 0; i <= size.height; i++ )
 305         {
 306             for( j = 0; j <= size.width; j++, p_cur++ )
 307             {
 308                 p_cur->c = pyramida[i * roi.width + j];
 309                 p_cur->p = &stub;
 310                 p_cur->a = 0;
 311                 p_cur->rect.x2 = 0;
 312             }
 313         }
 314     }
 315
 316     cvStartAppendToSeq( cmp_seq, &writer );
 317
 318     /* do several iterations to determine son-father links */
 319     for( cur_iter = 0; cur_iter < max_iter; cur_iter++ )
 320     {
 321         int is_last_iter = cur_iter == max_iter - 1;
 322
 323         size = roi;
 324
 325         /* build son-father links down up to level */
 326         for( l = 0; l < level; l++ )
 327         {
 328             icvUpdatePyrLinks_8u_C1( l, pyram[l], size, pyram[l + 1], &writer,
 329                                       (float) threshold1, is_last_iter, &stub,
 330                                       icvWritePyrNode );
 331
 332             /* clear last border row */
 333             if( l > 0 )
 334             {
 335                 p_cur = pyram[l] + (size.width + 1) * size.height;
 336                 for( j = 0; j <= size.width; j++ )
 337                     p_cur[j].c = 0;
 338             }
 339
 340             size.width >>= 1;
 341             size.height >>= 1;
 342         }
 343
 344 /*  clear the old c value for the last level     */
 345         p_cur = pyram[level];
 346         for( i = 0; i <= size.height; i++, p_cur += size.width + 1 )
 347             for( j = 0; j <= size.width; j++ )
 348                 p_cur[j].c = 0;
 349
 350         size = roi;
 351         step = roi.width;
 352
 353 /* calculate average c value for the 0 < l <=level   */
 354         for( l = 0; l < level; l++, step = (step >> 1) + 1 )
 355         {
 356             _CvPyramid *p_prev, *p_row_prev;
 357
 358             stub.c = 0;
 359
 360             /* calculate average c value for the next level   */
 361             if( l == 0 )
 362             {
 363                 p_base = (_CvPyramidBase *) pyram[0];
 364                 for( i = 0; i < roi.height; i++, p_base += size.width )
 365                 {
 366                     for( j = 0; j < size.width; j += 2 )
 367                     {
 368                         _CvPyramid *p1 = p_base[j].p;
 369                         _CvPyramid *p2 = p_base[j + 1].p;
 370
 371                         p1->c += p_base[j].c;
 372                         p2->c += p_base[j + 1].c;
 373                     }
 374                 }
 375             }
 376             else
 377             {
 378                 p_cur = pyram[l];
 379                 for( i = 0; i < size.height; i++, p_cur += size.width + 1 )
 380                 {
 381                     for( j = 0; j < size.width; j += 2 )
 382                     {
 383                         _CvPyramid *p1 = p_cur[j].p;
 384                         _CvPyramid *p2 = p_cur[j + 1].p;
 385
 386                         float t0 = (float) p_cur[j].a * p_cur[j].c;
 387                         float t1 = (float) p_cur[j + 1].a * p_cur[j + 1].c;
 388
 389                         p1->c += t0;
 390                         p2->c += t1;
 391
 392                         if( !is_last_iter )
 393                             p_cur[j].a = p_cur[j + 1].a = 0;
 394                     }
 395                     if( !is_last_iter )
 396                         p_cur[size.width].a = 0;
 397                 }
 398                 if( !is_last_iter )
 399                 {
 400                     for( j = 0; j <= size.width; j++ )
 401                     {
 402                         p_cur[j].a = 0;
 403                     }
 404                 }
 405             }
 406
 407             /* assign random values of the next level null c   */
 408             p_cur = pyram[l + 1];
 409             p_row_prev = p_prev = pyram[l];
 410
 411             size.width >>= 1;
 412             size.height >>= 1;
 413
 414             for( i = 0; i <= size.height; i++, p_cur += size.width + 1 )
 415             {
 416                 if( i < size.height || !is_last_iter )
 417                 {
 418                     for( j = 0; j < size.width; j++ )
 419                     {
 420                         int a = p_cur[j].a;
 421
 422                         if( a != 0 )
 423                         {
 424                             if( a <= _CV_INV_TAB_SIZE )
 425                             {
 426                                 p_cur[j].c *= icvInvTab[a - 1];
 427                             }
 428                             else
 429                             {
 430                                 p_cur[j].c /= a;
 431                             }
 432                         }
 433                         else
 434                         {
 435                             p_cur[j].c = p_prev->c;
 436                         }
 437
 438                         if( l == 0 )
 439                             p_prev = _CV_NEXT_BASE_C1(p_prev,2);
 440                         else
 441                             p_prev += 2;
 442                     }
 443
 444                     if( p_cur[size.width].a == 0 )
 445                     {
 446                         p_cur[size.width].c = p_prev[(l != 0) - 1].c;
 447                     }
 448                     else
 449                     {
 450                         p_cur[size.width].c /= p_cur[size.width].a;
 451                         if( is_last_iter )
 452                         {
 453                             cmp_node.data = p_cur + size.width;
 454                             CV_WRITE_SEQ_ELEM( cmp_node, writer );
 455                         }
 456                     }
 457                 }
 458                 else
 459                 {
 460                     for( j = 0; j <= size.width; j++ )
 461                     {
 462                         int a = p_cur[j].a;
 463
 464                         if( a != 0 )
 465                         {
 466                             if( a <= _CV_INV_TAB_SIZE )
 467                             {
 468                                 p_cur[j].c *= icvInvTab[a - 1];
 469                             }
 470                             else
 471                             {
 472                                 p_cur[j].c /= a;
 473                             }
 474
 475                             cmp_node.data = p_cur + j;
 476                             CV_WRITE_SEQ_ELEM( cmp_node, writer );
 477                         }
 478                         else
 479                         {
 480                             p_cur[j].c = p_prev->c;
 481                         }
 482
 483                         if( l == 0 )
 484                         {
 485                             p_prev = _CV_NEXT_BASE_C1(p_prev, (j * 2 < step - 2 ? 2 : 1));
 486                         }
 487                         else
 488                         {
 489                             p_prev++;
 490                         }
 491                     }
 492                 }
 493
 494                 if( l + 1 == level && !is_last_iter )
 495                     for( j = 0; j <= size.width; j++ )
 496                         p_cur[j].a = 0;
 497
 498                 if( !(i & 1) )
 499                 {
 500                     p_prev = p_row_prev;
 501                 }
 502                 else
 503                 {
 504                     p_prev = (_CvPyramid*)((char*)p_row_prev + step *
 505                         (l == 0 ? sizeof(_CvPyramidBase) : sizeof(_CvPyramid)));
 506                 }
 507             }
 508         }
 509     }                           /*  end of the iteration process  */
 510
 511     /* construct a connected  components   */
 512     size.width = roi.width >> level;
 513     size.height = roi.height >> level;
 514
 515     p_cur = pyram[level];
 516
 517     for( i = 0; i < size.height; i++, p_cur += size.width + 1 )
 518     {
 519         for( j = 0; j < size.width; j++ )
 520         {
 521             if( p_cur[j].a != 0 )
 522             {
 523                 cmp_node.data = p_cur + j;
 524                 CV_WRITE_SEQ_ELEM( cmp_node, writer );
 525             }
 526         }
 527     }
 528
 529     cvEndWriteSeq( &writer );
 530
 531 /* clusterization segmented components and construction
 532    output connected components                            */
 533     icvSegmentClusterC1( cmp_seq, res_seq, threshold2, pyram[1], roi );
 534
 535 /* convert (inplace) resultant segment values to int (top level) */
 536
 537 /* propagate segment values top down */
 538     for( l = level - 1; l >= 0; l-- )
 539     {
 540         p_cur = pyram[l];
 541
 542         size.width <<= 1;
 543         size.height <<= 1;
 544
 545         if( l == 0 )
 546         {
 547             size.width--;
 548             size.height--;
 549         }
 550
 551         for( i = 0; i <= size.height; i++ )
 552         {
 553             for( j = 0; j <= size.width; j++ )
 554             {
 555                 _CvPyramid *p = p_cur->p;
 556
 557                 assert( p != 0 );
 558                 if( p != &stub )
 559                     p_cur->c = p->c;
 560
 561                 if( l == 0 )
 562                 {
 563                     Cv32suf _c;
 564                     /* copy the segmented values to destination image */
 565                     _c.f = p_cur->c; dst_image[j] = (uchar)_c.i;
 566                     p_cur = _CV_NEXT_BASE_C1(p_cur, 1);
 567                 }
 568                 else
 569                 {
 570                     p_cur++;
 571                 }
 572             }
 573             if( l == 0 )
 574                 dst_image += dst_step;
 575         }
 576     }
 577   M_END:
 578
 579     cvFree( &buffer );
 580     cvReleaseMemStorage( &temp_storage );
 581
 582     if( status == CV_OK )
 583         *dst_comp = res_seq;
 584
 585     return status;
 586 }
 587
 588
 589
 590 /****************************************************************************************\
 591     color!!!  image segmentation by pyramid-linking
 592 \****************************************************************************************/
 593 static CvStatus
 594 icvPyrSegmentation8uC3R( uchar * src_image, int src_step,
 595                          uchar * dst_image, int dst_step,
 596                          CvSize roi, CvFilter filter,
 597                          CvSeq ** dst_comp, CvMemStorage * storage,
 598                          int level, int threshold1, int threshold2 )
 599 {
 600     int i, j, l;
 601
 602     int step;
 603     const int max_iter = 3;     /* maximum number of iterations */
 604     int cur_iter = 0;           /* current iteration */
 605
 606     _CvPyramidC3 *pyram[16];    /* pointers to the pyramid down up to level */
 607
 608     float *pyramida = 0;
 609     _CvPyramidC3 stub;
 610
 611     _CvPyramidC3 *p_cur;
 612     _CvPyramidBaseC3 *p_base;
 613     _CvListNode cmp_node;
 614
 615     CvSeq *cmp_seq = 0;
 616     CvSeq *res_seq = 0;
 617     CvMemStorage *temp_storage = 0;
 618     CvSize size;
 619     CvStatus status;
 620     CvSeqWriter writer;
 621
 622     int buffer_size;
 623     char *buffer = 0;
 624
 625     status = CV_OK;
 626
 627     threshold1 *= _CV_RGB_THRESH_SCALE;
 628     threshold2 *= _CV_RGB_THRESH_SCALE;
 629
 630     /* clear pointer to resultant sequence */
 631     if( dst_comp )
 632         *dst_comp = 0;
 633
 634     /* check args */
 635     if( !src_image || !dst_image || !storage || !dst_comp )
 636         return CV_NULLPTR_ERR;
 637     if( roi.width <= 0 || roi.height <= 0 ||
 638         src_step < roi.width * 3 || dst_step < roi.width * 3 ) return CV_BADSIZE_ERR;
 639     if( filter != CV_GAUSSIAN_5x5 )
 640         return CV_BADRANGE_ERR;
 641     if( threshold1 < 0 || threshold2 < 0 )
 642         return CV_BADRANGE_ERR;
 643     if( level <= 0 )
 644         return CV_BADRANGE_ERR;
 645
 646     if( ((roi.width | roi.height) & ((1 << level) - 1)) != 0 )
 647         return CV_BADCOEF_ERR;
 648
 649     temp_storage = cvCreateChildMemStorage( storage );
 650
 651     /* sequence for temporary components */
 652     cmp_seq = cvCreateSeq( 0, sizeof( CvSeq ), sizeof( _CvListNode ), temp_storage );
 653     assert( cmp_seq != 0 );
 654
 655     res_seq = cvCreateSeq( CV_SEQ_CONNECTED_COMP, sizeof( CvSeq ),
 656                            sizeof( CvConnectedComp ), storage );
 657     assert( res_seq != 0 );
 658
 659     /* calculate buffer size */
 660     buffer_size = roi.width * roi.height * (sizeof( _CvRGBf ) + sizeof( _CvPyramidBaseC3 ));
 661
 662     for( l = 1; l <= level; l++ )
 663         buffer_size += ((roi.width >> l) + 1) * ((roi.height >> l) + 1) * sizeof(_CvPyramidC3);
 664
 665     /* allocate buffer */
 666     buffer = (char *) cvAlloc( buffer_size );
 667     if( !buffer )
 668     {
 669         status = CV_OUTOFMEM_ERR;
 670         goto M_END;
 671     }
 672
 673     pyramida = (float *) buffer;
 674
 675     /* initialization pyramid-linking properties down up to level */
 676     step = roi.width * sizeof( _CvRGBf );
 677
 678     {
 679         CvMat _src;
 680         CvMat _pyramida;
 681         cvInitMatHeader( &_src, roi.height, roi.width, CV_8UC3, src_image, src_step );
 682         cvInitMatHeader( &_pyramida, roi.height, roi.width, CV_32FC3, pyramida, step );
 683         cvConvert( &_src, &_pyramida );
 684         /*_CV_CHECK( icvCvtTo_32f_C1R( src_image, src_step, pyramida, step,
 685                                  cvSize( roi.width * 3, roi.height ), CV_8UC1 ));*/
 686     }
 687
 688     p_base = (_CvPyramidBaseC3 *) (buffer + step * roi.height);
 689     pyram[0] = (_CvPyramidC3 *) p_base;
 690
 691     /* fill base level of pyramid */
 692     for( i = 0; i < roi.height; i++ )
 693     {
 694         for( j = 0; j < roi.width; j++, p_base++ )
 695         {
 696             p_base->c = ((_CvRGBf *) pyramida)[i * roi.width + j];
 697             p_base->p = &stub;
 698         }
 699     }
 700
 701     p_cur = (_CvPyramidC3 *) p_base;
 702     size = roi;
 703
 704     /* calculate initial pyramid */
 705     for( l = 1; l <= level; l++ )
 706     {
 707         CvSize dst_size = { size.width/2 + 1, size.height/2 + 1 };
 708         CvMat prev_level = cvMat( size.height, size.width, CV_32FC3 );
 709         CvMat next_level = cvMat( dst_size.height, dst_size.width, CV_32FC3 );
 710
 711         cvSetData( &prev_level, pyramida, step );
 712         cvSetData( &next_level, pyramida, step );
 713         cvPyrDown( &prev_level, &next_level );
 714
 715         //_CV_CHECK( icvPyrDown_Gauss5x5_32f_C3R( pyramida, step, pyramida, step, size, buff ));
 716         //_CV_CHECK( icvPyrDownBorder_32f_CnR( pyramida, step, size, pyramida, step, dst_size, 3 ));
 717         pyram[l] = p_cur;
 718
 719         size.width = dst_size.width - 1;
 720         size.height = dst_size.height - 1;
 721
 722         /* fill layer #l */
 723         for( i = 0; i <= size.height; i++ )
 724         {
 725             assert( (char*)p_cur - buffer < buffer_size );
 726             for( j = 0; j <= size.width; j++, p_cur++ )
 727             {
 728                 p_cur->c = ((_CvRGBf *) pyramida)[i * roi.width + j];
 729                 p_cur->p = &stub;
 730                 p_cur->a = 0;
 731                 p_cur->rect.x2 = 0;
 732             }
 733         }
 734     }
 735
 736     cvStartAppendToSeq( cmp_seq, &writer );
 737
 738     /* do several iterations to determine son-father links */
 739     for( cur_iter = 0; cur_iter < max_iter; cur_iter++ )
 740     {
 741         int is_last_iter = cur_iter == max_iter - 1;
 742
 743         size = roi;
 744
 745         /* build son-father links down up to level */
 746         for( l = 0; l < level; l++ )
 747         {
 748             icvUpdatePyrLinks_8u_C3( l, pyram[l], size, pyram[l + 1], &writer,
 749                                       (float) threshold1, is_last_iter, &stub,
 750                                       icvWritePyrNode );
 751
 752             /* clear last border row */
 753             if( l > 0 )
 754             {
 755                 p_cur = pyram[l] + (size.width + 1) * size.height;
 756                 for( j = 0; j <= size.width; j++ )
 757                     p_cur[j].c.blue = p_cur[j].c.green = p_cur[j].c.red = 0;
 758             }
 759
 760             size.width >>= 1;
 761             size.height >>= 1;
 762         }
 763
 764 /*  clear the old c value for the last level     */
 765         p_cur = pyram[level];
 766         for( i = 0; i <= size.height; i++, p_cur += size.width + 1 )
 767             for( j = 0; j <= size.width; j++ )
 768                 p_cur[j].c.blue = p_cur[j].c.green = p_cur[j].c.red = 0;
 769
 770         size = roi;
 771         step = roi.width;
 772
 773 /* calculate average c value for the 0 < l <=level   */
 774         for( l = 0; l < level; l++, step = (step >> 1) + 1 )
 775         {
 776             _CvPyramidC3 *p_prev, *p_row_prev;
 777
 778             stub.c.blue = stub.c.green = stub.c.red = 0;
 779
 780             /* calculate average c value for the next level   */
 781             if( l == 0 )
 782             {
 783                 p_base = (_CvPyramidBaseC3 *) pyram[0];
 784                 for( i = 0; i < roi.height; i++, p_base += size.width )
 785                 {
 786                     for( j = 0; j < size.width; j++ )
 787                     {
 788                         _CvPyramidC3 *p = p_base[j].p;
 789
 790                         p->c.blue += p_base[j].c.blue;
 791                         p->c.green += p_base[j].c.green;
 792                         p->c.red += p_base[j].c.red;
 793                     }
 794                 }
 795             }
 796             else
 797             {
 798                 p_cur = pyram[l];
 799                 for( i = 0; i < size.height; i++, p_cur += size.width + 1 )
 800                 {
 801                     for( j = 0; j < size.width; j++ )
 802                     {
 803                         _CvPyramidC3 *p = p_cur[j].p;
 804                         float a = (float) p_cur[j].a;
 805
 806                         p->c.blue += a * p_cur[j].c.blue;
 807                         p->c.green += a * p_cur[j].c.green;
 808                         p->c.red += a * p_cur[j].c.red;
 809
 810                         if( !is_last_iter )
 811                             p_cur[j].a = 0;
 812                     }
 813                     if( !is_last_iter )
 814                         p_cur[size.width].a = 0;
 815                 }
 816                 if( !is_last_iter )
 817                 {
 818                     for( j = 0; j <= size.width; j++ )
 819                     {
 820                         p_cur[j].a = 0;
 821                     }
 822                 }
 823             }
 824
 825             /* assign random values of the next level null c   */
 826             p_cur = pyram[l + 1];
 827             p_row_prev = p_prev = pyram[l];
 828
 829             size.width >>= 1;
 830             size.height >>= 1;
 831
 832             for( i = 0; i <= size.height; i++, p_cur += size.width + 1 )
 833             {
 834                 if( i < size.height || !is_last_iter )
 835                 {
 836                     for( j = 0; j < size.width; j++ )
 837                     {
 838                         int a = p_cur[j].a;
 839
 840                         if( a != 0 )
 841                         {
 842                             float inv_a;
 843
 844                             if( a <= _CV_INV_TAB_SIZE )
 845                             {
 846                                 inv_a = icvInvTab[a - 1];
 847                             }
 848                             else
 849                             {
 850                                 inv_a = 1.f / a;
 851                             }
 852                             p_cur[j].c.blue *= inv_a;
 853                             p_cur[j].c.green *= inv_a;
 854                             p_cur[j].c.red *= inv_a;
 855                         }
 856                         else
 857                         {
 858                             p_cur[j].c = p_prev->c;
 859                         }
 860
 861                         if( l == 0 )
 862                             p_prev = _CV_NEXT_BASE_C3( p_prev, 2 );
 863                         else
 864                             p_prev += 2;
 865                     }
 866
 867                     if( p_cur[size.width].a == 0 )
 868                     {
 869                         p_cur[size.width].c = p_prev[(l != 0) - 1].c;
 870                     }
 871                     else
 872                     {
 873                         p_cur[size.width].c.blue /= p_cur[size.width].a;
 874                         p_cur[size.width].c.green /= p_cur[size.width].a;
 875                         p_cur[size.width].c.red /= p_cur[size.width].a;
 876                         if( is_last_iter )
 877                         {
 878                             cmp_node.data = p_cur + size.width;
 879                             CV_WRITE_SEQ_ELEM( cmp_node, writer );
 880                         }
 881                     }
 882                 }
 883                 else
 884                 {
 885                     for( j = 0; j <= size.width; j++ )
 886                     {
 887                         int a = p_cur[j].a;
 888
 889                         if( a != 0 )
 890                         {
 891                             float inv_a;
 892
 893                             if( a <= _CV_INV_TAB_SIZE )
 894                             {
 895                                 inv_a = icvInvTab[a - 1];
 896                             }
 897                             else
 898                             {
 899                                 inv_a = 1.f / a;
 900                             }
 901                             p_cur[j].c.blue *= inv_a;
 902                             p_cur[j].c.green *= inv_a;
 903                             p_cur[j].c.red *= inv_a;
 904
 905                             cmp_node.data = p_cur + j;
 906                             CV_WRITE_SEQ_ELEM( cmp_node, writer );
 907                         }
 908                         else
 909                         {
 910                             p_cur[j].c = p_prev->c;
 911                         }
 912
 913                         if( l == 0 )
 914                         {
 915                             p_prev = _CV_NEXT_BASE_C3( p_prev, (j * 2 < step - 2 ? 2 : 1));
 916                         }
 917                         else
 918                         {
 919                             p_prev++;
 920                         }
 921                     }
 922                 }
 923
 924                 if( l + 1 == level && !is_last_iter )
 925                     for( j = 0; j <= size.width; j++ )
 926                         p_cur[j].a = 0;
 927
 928                 if( !(i & 1) )
 929                 {
 930                     p_prev = p_row_prev;
 931                 }
 932                 else
 933                 {
 934                     p_prev = (_CvPyramidC3*)((char*)p_row_prev + step *
 935                         (l == 0 ? sizeof( _CvPyramidBaseC3 ) : sizeof( _CvPyramidC3 )));
 936                 }
 937             }
 938         }
 939     }                           /*  end of the iteration process  */
 940
 941     /* construct a connected  components   */
 942     size.width = roi.width >> level;
 943     size.height = roi.height >> level;
 944
 945     p_cur = pyram[level];
 946
 947     for( i = 0; i < size.height; i++, p_cur += size.width + 1 )
 948     {
 949         for( j = 0; j < size.width; j++ )
 950         {
 951             if( p_cur[j].a != 0 )
 952             {
 953                 cmp_node.data = p_cur + j;
 954                 CV_WRITE_SEQ_ELEM( cmp_node, writer );
 955             }
 956         }
 957     }
 958
 959     cvEndWriteSeq( &writer );
 960
 961 /* clusterization segmented components and construction
 962    output connected components                            */
 963     icvSegmentClusterC3( cmp_seq, res_seq, threshold2, pyram[1], roi );
 964
 965 /* convert (inplace) resultant segment values to int (top level) */
 966
 967 /* propagate segment values top down */
 968     for( l = level - 1; l >= 0; l-- )
 969     {
 970         p_cur = pyram[l];
 971
 972         size.width <<= 1;
 973         size.height <<= 1;
 974
 975         if( l == 0 )
 976         {
 977             size.width--;
 978             size.height--;
 979         }
 980
 981         for( i = 0; i <= size.height; i++ )
 982         {
 983             for( j = 0; j <= size.width; j++ )
 984             {
 985                 _CvPyramidC3 *p = p_cur->p;
 986
 987                 assert( p != 0 );
 988                 if( p != &stub )
 989                 {
 990                     p_cur->c = p->c;
 991                 }
 992
 993                 if( l == 0 )
 994                 {
 995                     Cv32suf _c;
 996                     /* copy the segmented values to destination image */
 997                     _c.f = p_cur->c.blue; dst_image[j*3] = (uchar)_c.i;
 998                     _c.f = p_cur->c.green; dst_image[j*3+1] = (uchar)_c.i;
 999                     _c.f = p_cur->c.red; dst_image[j*3+2] = (uchar)_c.i;
1000                     p_cur = _CV_NEXT_BASE_C3(p_cur,1);
1001                 }
1002                 else
1003                 {
1004                     p_cur++;
1005                 }
1006             }
1007             if( l == 0 )
1008                 dst_image += dst_step;
1009         }
1010     }
1011
1012   M_END:
1013
1014     cvFree( &buffer );
1015     cvReleaseMemStorage( &temp_storage );
1016
1017     if( status == CV_OK )
1018         *dst_comp = res_seq;
1019
1020     return status;
1021 }
1022
1023
1024 static CvStatus icvUpdatePyrLinks_8u_C1
1025     (int layer, void *layer_data, CvSize size, void *parent_layer,
1026      void *_writer, float threshold, int is_last_iter, void *_stub, CvWriteNodeFunction /*func*/)
1027 {
1028     int i, j;
1029     _CvListNode cmp_node;
1030
1031     _CvPyramid *stub = (_CvPyramid *) _stub;
1032     _CvPyramid *p_cur = (_CvPyramid *) layer_data;
1033     _CvPyramid *p_next1 = (_CvPyramid *) parent_layer;
1034     _CvPyramid *p_next3 = p_next1 + (size.width >> 1) + 1;
1035
1036     CvSeqWriter & writer = *(CvSeqWriter *) _writer;
1037
1038     for( i = 0; i < size.height; i++ )
1039     {
1040         for( j = 0; j < size.width; j += 2 )
1041         {
1042             float c0, c1, c2, c3, c4;
1043             _CvPyramid *p;
1044
1045 /* son-father threshold linking for the current node establish */
1046             c0 = p_cur->c;
1047
1048 /* find pointer for the first pixel */
1049             c1 = (float) fabs( c0 - p_next1[0].c );
1050             c2 = (float) fabs( c0 - p_next1[1].c );
1051             c3 = (float) fabs( c0 - p_next3[0].c );
1052             c4 = (float) fabs( c0 - p_next3[1].c );
1053
1054             p = p_next1;
1055
1056             if( c1 > c2 )
1057             {
1058                 p = p_next1 + 1;
1059                 c1 = c2;
1060             }
1061             if( c1 > c3 )
1062             {
1063                 p = p_next3;
1064                 c1 = c3;
1065             }
1066             if( c1 > c4 )
1067             {
1068                 p = p_next3 + 1;
1069                 c1 = c4;
1070             }
1071
1072             if( c1 <= threshold )
1073             {
1074                 p_cur->p = p;
1075
1076                 if( layer == 0 )
1077                 {
1078                     p->a++;
1079                     p_cur = (_CvPyramid*)((char*)p_cur + sizeof(_CvPyramidBase));
1080                     if( is_last_iter )
1081                         icvMaxRoi1( &(p->rect), j, i );
1082                 }
1083                 else
1084                 {
1085                     int a = p_cur->a;
1086
1087                     p->a += a;
1088                     p_cur->c = 0;
1089                     p_cur++;
1090                     if( is_last_iter && a != 0 )
1091                         icvMaxRoi( &(p->rect), &(p_cur[-1].rect) );
1092                 }
1093             }
1094             else
1095             {
1096                 p_cur->p = stub;
1097                 if( is_last_iter )
1098                 {
1099                     cmp_node.data = p_cur;
1100                     CV_WRITE_SEQ_ELEM( cmp_node, writer );
1101                 }
1102                 if( layer == 0 )
1103                 {
1104                     p_cur = _CV_NEXT_BASE_C1(p_cur,1);
1105                 }
1106                 else
1107                 {
1108                     p_cur->c = 0;
1109                     p_cur++;
1110                 }
1111             }
1112
1113             /* find pointer for the second pixel */
1114             c0 = p_cur->c;
1115
1116             c1 = (float) fabs( c0 - p_next1[0].c );
1117             c2 = (float) fabs( c0 - p_next1[1].c );
1118             c3 = (float) fabs( c0 - p_next3[0].c );
1119             c4 = (float) fabs( c0 - p_next3[1].c );
1120
1121             p = p_next1;
1122             p_next1++;
1123
1124             if( c1 > c2 )
1125             {
1126                 p = p_next1;
1127                 c1 = c2;
1128             }
1129             if( c1 > c3 )
1130             {
1131                 p = p_next3;
1132                 c1 = c3;
1133             }
1134
1135             p_next3++;
1136             if( c1 > c4 )
1137             {
1138                 p = p_next3;
1139                 c1 = c4;
1140             }
1141
1142             if( c1 <= threshold )
1143             {
1144                 p_cur->p = p;
1145
1146                 if( layer == 0 )
1147                 {
1148                     p->a++;
1149                     p_cur = _CV_NEXT_BASE_C1(p_cur,1);
1150                     if( is_last_iter )
1151                         icvMaxRoi1( &(p->rect), j + 1, i );
1152                 }
1153                 else
1154                 {
1155                     int a = p_cur->a;
1156
1157                     p->a += a;
1158                     p_cur->c = 0;
1159                     p_cur++;
1160                     if( is_last_iter && a != 0 )
1161                         icvMaxRoi( &(p->rect), &(p_cur[-1].rect) );
1162                 }
1163             }
1164             else
1165             {
1166                 p_cur->p = stub;
1167                 if( is_last_iter )
1168                 {
1169                     cmp_node.data = p_cur;
1170                     CV_WRITE_SEQ_ELEM( cmp_node, writer );
1171                 }
1172                 if( layer == 0 )
1173                 {
1174                     p_cur = _CV_NEXT_BASE_C1(p_cur,1);
1175                 }
1176                 else
1177                 {
1178                     p_cur->c = 0;
1179                     p_cur++;
1180                 }
1181             }
1182         }
1183
1184         /* clear c's */
1185         if( layer > 0 )
1186         {
1187             p_cur->c = 0;
1188             p_cur++;
1189         }
1190
1191         if( !(i & 1) )
1192         {
1193             p_next1 -= size.width >> 1;
1194             p_next3 -= size.width >> 1;
1195         }
1196         else
1197         {
1198             p_next1++;
1199             p_next3++;
1200         }
1201     }
1202
1203     return CV_OK;
1204 }
1205
1206
1207 static CvStatus icvUpdatePyrLinks_8u_C3
1208     (int layer, void *layer_data, CvSize size, void *parent_layer,
1209      void *_writer, float threshold, int is_last_iter, void *_stub, CvWriteNodeFunction /*func*/)
1210 {
1211     int i, j;
1212     _CvListNode cmp_node;
1213
1214     _CvPyramidC3 *stub = (_CvPyramidC3 *) _stub;
1215     _CvPyramidC3 *p_cur = (_CvPyramidC3 *) layer_data;
1216     _CvPyramidC3 *p_next1 = (_CvPyramidC3 *) parent_layer;
1217     _CvPyramidC3 *p_next3 = p_next1 + (size.width >> 1) + 1;
1218
1219     CvSeqWriter & writer = *(CvSeqWriter *) _writer;
1220
1221     for( i = 0; i < size.height; i++ )
1222     {
1223         for( j = 0; j < size.width; j += 2 )
1224         {
1225             float c1, c2, c3, c4;
1226             _CvPyramidC3 *p;
1227
1228 /* find pointer for the first pixel */
1229             c1 = _CV_RGB_DIST( p_cur->c, p_next1[0].c );
1230             c2 = _CV_RGB_DIST( p_cur->c, p_next1[1].c );
1231             c3 = _CV_RGB_DIST( p_cur->c, p_next3[0].c );
1232             c4 = _CV_RGB_DIST( p_cur->c, p_next3[1].c );
1233
1234             p = p_next1;
1235
1236             if( c1 > c2 )
1237             {
1238                 p = p_next1 + 1;
1239                 c1 = c2;
1240             }
1241             if( c1 > c3 )
1242             {
1243                 p = p_next3;
1244                 c1 = c3;
1245             }
1246             if( c1 > c4 )
1247             {
1248                 p = p_next3 + 1;
1249                 c1 = c4;
1250             }
1251
1252             if( c1 < threshold )
1253             {
1254                 p_cur->p = p;
1255
1256                 if( layer == 0 )
1257                 {
1258                     p->a++;
1259                     p_cur = _CV_NEXT_BASE_C3(p_cur,1);
1260                     if( is_last_iter )
1261                         icvMaxRoi1( &(p->rect), j, i );
1262                 }
1263                 else
1264                 {
1265                     int a = p_cur->a;
1266
1267                     p->a += a;
1268                     p_cur->c.blue = p_cur->c.green = p_cur->c.red = 0;
1269                     p_cur++;
1270                     if( is_last_iter && a != 0 )
1271                         icvMaxRoi( &(p->rect), &(p_cur[-1].rect) );
1272                 }
1273             }
1274             else
1275             {
1276                 p_cur->p = stub;
1277                 if( is_last_iter /* && ( == 0 || p_cur->a != 0) */  )
1278                 {
1279                     cmp_node.data = p_cur;
1280                     CV_WRITE_SEQ_ELEM( cmp_node, writer );
1281                 }
1282
1283                 if( layer == 0 )
1284                 {
1285                     p_cur = _CV_NEXT_BASE_C3(p_cur,1);
1286                 }
1287                 else
1288                 {
1289                     p_cur->c.blue = p_cur->c.green = p_cur->c.red = 0;
1290                     p_cur++;
1291                 }
1292             }
1293
1294             /* find pointer for the second pixel */
1295             c1 = _CV_RGB_DIST( p_cur->c, p_next1[0].c );
1296             c2 = _CV_RGB_DIST( p_cur->c, p_next1[1].c );
1297             c3 = _CV_RGB_DIST( p_cur->c, p_next3[0].c );
1298             c4 = _CV_RGB_DIST( p_cur->c, p_next3[1].c );
1299
1300             p = p_next1;
1301             p_next1++;
1302
1303             if( c1 > c2 )
1304             {
1305                 p = p_next1;
1306                 c1 = c2;
1307             }
1308             if( c1 > c3 )
1309             {
1310                 p = p_next3;
1311                 c1 = c3;
1312             }
1313
1314             p_next3++;
1315             if( c1 > c4 )
1316             {
1317                 p = p_next3;
1318                 c1 = c4;
1319             }
1320
1321             if( c1 < threshold )
1322             {
1323                 p_cur->p = p;
1324
1325                 if( layer == 0 )
1326                 {
1327                     p->a++;
1328                     p_cur = _CV_NEXT_BASE_C3(p_cur,1);
1329                     if( is_last_iter )
1330                         icvMaxRoi1( &(p->rect), j + 1, i );
1331                 }
1332                 else
1333                 {
1334                     int a = p_cur->a;
1335
1336                     p->a += a;
1337                     p_cur->c.blue = p_cur->c.green = p_cur->c.red = 0;
1338                     p_cur++;
1339                     if( is_last_iter && a != 0 )
1340                         icvMaxRoi( &(p->rect), &(p_cur[-1].rect) );
1341                 }
1342             }
1343             else
1344             {
1345                 p_cur->p = stub;
1346                 if( is_last_iter /* && ( == 0 || p_cur->a != 0) */  )
1347                 {
1348                     cmp_node.data = p_cur;
1349                     CV_WRITE_SEQ_ELEM( cmp_node, writer );
1350                 }
1351                 if( layer == 0 )
1352                 {
1353                     p_cur = _CV_NEXT_BASE_C3(p_cur,1);
1354                 }
1355                 else
1356                 {
1357                     p_cur->c.blue = p_cur->c.green = p_cur->c.red = 0;
1358                     p_cur++;
1359                 }
1360             }
1361         }
1362
1363         /* clear c's */
1364         if( layer > 0 )
1365         {
1366             p_cur->c.blue = p_cur->c.green = p_cur->c.red = 0;
1367             p_cur++;
1368         }
1369
1370         if( !(i & 1) )
1371         {
1372             p_next1 -= size.width >> 1;
1373             p_next3 -= size.width >> 1;
1374         }
1375         else
1376         {
1377             p_next1++;
1378             p_next3++;
1379         }
1380     }
1381
1382     return CV_OK;
1383 }
1384
1385
1386
1387 /****************************************************************************************\
1388
1389     clusterization segmented components
1390
1391 \****************************************************************************************/
1392 static void
1393 icvExpandBaseLevelC1( _CvPyramid * base_p, _CvPyramid * p, _CvPyramidBase * start, int width )
1394 {
1395     int x = (int)((_CvPyramidBase *) base_p - start);
1396     int y = x / width;
1397
1398     x -= y * width;
1399     p->a = 1;
1400     p->rect.x1 = (ushort) x;
1401     p->rect.y1 = (ushort) y;
1402     p->rect.x2 = (ushort) (x + 1);
1403     p->rect.y2 = (ushort) (y + 1);
1404     p->c = base_p->c;
1405 }
1406
1407 CvStatus
1408 icvSegmentClusterC1( CvSeq * cmp_seq, CvSeq * res_seq,
1409                      double threshold, _CvPyramid * first_level_end, CvSize first_level_size )
1410 {
1411     const double eps = 1.;
1412     CvSeqWriter writer;
1413     CvSeqReader reader;
1414     _CvPyramid temp_cmp;
1415     _CvPyramidBase *first_level_start = (_CvPyramidBase *) first_level_end -
1416         first_level_size.width * first_level_size.height;
1417     int c, i, count = cmp_seq->total;
1418
1419     cvStartReadSeq( cmp_seq, &reader, 0 );
1420     cvStartAppendToSeq( res_seq, &writer );
1421
1422     if( threshold < eps )
1423     {
1424         /* if threshold is too small then simply copy all
1425            the components to the output sequence */
1426         for( i = 0; i < count; i++ )
1427         {
1428             CvConnectedComp comp;
1429             _CvPyramid *cmp = (_CvPyramid *) (((_CvListNode *) reader.ptr)->data);
1430             Cv32suf _c;
1431
1432             if( cmp < first_level_end )
1433             {
1434                 icvExpandBaseLevelC1( cmp, &temp_cmp, first_level_start,
1435                                       first_level_size.width );
1436                 cmp = &temp_cmp;
1437             }
1438
1439             _c.i = cvRound( cmp->c );
1440             cmp->c = _c.f;
1441             comp.value = cvRealScalar(_c.i);
1442             comp.area = cmp->a;
1443             comp.rect.x = cmp->rect.x1;
1444             comp.rect.y = cmp->rect.y1;
1445             comp.rect.width = cmp->rect.x2 - cmp->rect.x1;
1446             comp.rect.height = cmp->rect.y2 - cmp->rect.y1;
1447             comp.contour = 0;
1448
1449             CV_WRITE_SEQ_ELEM( comp, writer );
1450             CV_NEXT_SEQ_ELEM( sizeof( _CvListNode ), reader );
1451         }
1452     }
1453     else
1454     {
1455         _CvListNode stub_node;
1456         _CvListNode *prev = &stub_node;
1457
1458         stub_node.next = 0;
1459
1460         for( i = 0; i < count; i++ )
1461         {
1462             _CvListNode *node = (_CvListNode *) reader.ptr;
1463
1464             prev->next = node;
1465             prev = node;
1466             CV_NEXT_SEQ_ELEM( sizeof( _CvListNode ), reader );
1467         }
1468         prev->next = 0;
1469         prev = stub_node.next;
1470
1471         while( prev )
1472         {
1473             _CvListNode *node = prev->next;
1474             _CvListNode *acc = prev;
1475             _CvPyramid *cmp = (_CvPyramid *) (acc->data);
1476             CvConnectedComp comp;
1477             float c0 = cmp->c;
1478
1479             if( cmp < first_level_end )
1480             {
1481                 icvExpandBaseLevelC1( cmp, &temp_cmp, first_level_start,
1482                                       first_level_size.width );
1483             }
1484             else
1485             {
1486                 temp_cmp = *cmp;
1487                 temp_cmp.c *= temp_cmp.a;
1488             }
1489
1490             acc->next = 0;
1491             stub_node.next = 0;
1492             prev = &stub_node;
1493
1494             while( node )
1495             {
1496                 cmp = (_CvPyramid *) (node->data);
1497                 if( fabs( c0 - cmp->c ) < threshold )
1498                 {
1499                     _CvPyramid temp;
1500
1501                     /* exclude from global list and add to list of joint component */
1502                     prev->next = node->next;
1503                     node->next = acc;
1504                     acc = node;
1505
1506                     if( cmp < first_level_end )
1507                     {
1508                         icvExpandBaseLevelC1( cmp, &temp, first_level_start,
1509                                               first_level_size.width );
1510                         cmp = &temp;
1511                     }
1512
1513                     temp_cmp.a += cmp->a;
1514                     temp_cmp.c += cmp->c * cmp->a;
1515                     icvMaxRoi( &(temp_cmp.rect), &(cmp->rect) );
1516                 }
1517                 else
1518                 {
1519                     if( prev == &stub_node )
1520                     {
1521                         stub_node.next = node;
1522                     }
1523                     prev = node;
1524                 }
1525                 node = prev->next;
1526             }
1527
1528             if( temp_cmp.a != 0 )
1529             {
1530                 c = cvRound( temp_cmp.c / temp_cmp.a );
1531             }
1532             else
1533             {
1534                 c = cvRound( c0 );
1535             }
1536             node = acc;
1537
1538             while( node )
1539             {
1540                 Cv32suf _c;
1541                 cmp = (_CvPyramid *) (node->data);
1542                 _c.i = c; cmp->c = _c.f;
1543                 node = node->next;
1544             }
1545
1546             comp.value = cvRealScalar(c);
1547             comp.area = temp_cmp.a;
1548             comp.rect.x = temp_cmp.rect.x1;
1549             comp.rect.y = temp_cmp.rect.y1;
1550             comp.rect.width = temp_cmp.rect.x2 - temp_cmp.rect.x1;
1551             comp.rect.height = temp_cmp.rect.y2 - temp_cmp.rect.y1;
1552             comp.contour = 0;
1553
1554             CV_WRITE_SEQ_ELEM( comp, writer );
1555             prev = stub_node.next;
1556         }
1557     }
1558
1559     cvEndWriteSeq( &writer );
1560     return CV_OK;
1561 }
1562
1563 /****************************************************************************************\
1564
1565     clusterization segmented components
1566
1567 \****************************************************************************************/
1568 static void
1569 icvExpandBaseLevelC3( _CvPyramidC3 * base_p, _CvPyramidC3 * p,
1570                       _CvPyramidBaseC3 * start, int width )
1571 {
1572     int x = (int)((_CvPyramidBaseC3 *) base_p - start);
1573     int y = x / width;
1574
1575     x -= y * width;
1576     p->a = 1;
1577     p->rect.x1 = (ushort) x;
1578     p->rect.y1 = (ushort) y;
1579     p->rect.x2 = (ushort) (x + 1);
1580     p->rect.y2 = (ushort) (y + 1);
1581     p->c = base_p->c;
1582 }
1583
1584 CvStatus
1585 icvSegmentClusterC3( CvSeq * cmp_seq, CvSeq * res_seq,
1586                      double threshold,
1587                      _CvPyramidC3 * first_level_end, CvSize first_level_size )
1588 {
1589     const double eps = 1.;
1590     CvSeqWriter writer;
1591     CvSeqReader reader;
1592     _CvPyramidC3 temp_cmp;
1593     _CvPyramidBaseC3 *first_level_start = (_CvPyramidBaseC3 *) first_level_end -
1594         first_level_size.width * first_level_size.height;
1595     int i, count = cmp_seq->total;
1596     int c_blue, c_green, c_red;
1597
1598     cvStartReadSeq( cmp_seq, &reader, 0 );
1599     cvStartAppendToSeq( res_seq, &writer );
1600
1601     if( threshold < eps )
1602     {
1603         /* if threshold is too small then simply copy all
1604            the components to the output sequence */
1605         for( i = 0; i < count; i++ )
1606         {
1607             CvConnectedComp comp;
1608             _CvPyramidC3 *cmp = (_CvPyramidC3 *) (((_CvListNode *) reader.ptr)->data);
1609             Cv32suf _c;
1610
1611             if( cmp < first_level_end )
1612             {
1613                 icvExpandBaseLevelC3( cmp, &temp_cmp, first_level_start,
1614                                       first_level_size.width );
1615                 cmp = &temp_cmp;
1616             }
1617
1618             c_blue = cvRound( cmp->c.blue );
1619             c_green = cvRound( cmp->c.green );
1620             c_red = cvRound( cmp->c.red );
1621             _c.i = c_blue; cmp->c.blue = _c.f;
1622             _c.i = c_green; cmp->c.green = _c.f;
1623             _c.i = c_red; cmp->c.red = _c.f;
1624             comp.value = cvScalar( c_blue, c_green, c_red );
1625             comp.area = cmp->a;
1626             comp.rect.x = cmp->rect.x1;
1627             comp.rect.y = cmp->rect.y1;
1628             comp.rect.width = cmp->rect.x2 - cmp->rect.x1;
1629             comp.rect.height = cmp->rect.y2 - cmp->rect.y1;
1630             comp.contour = 0;
1631
1632             CV_WRITE_SEQ_ELEM( comp, writer );
1633             CV_NEXT_SEQ_ELEM( sizeof( _CvListNode ), reader );
1634         }
1635     }
1636     else
1637     {
1638         _CvListNode stub_node;
1639         _CvListNode *prev = &stub_node;
1640
1641         stub_node.next = 0;
1642
1643         for( i = 0; i < count; i++ )
1644         {
1645             _CvListNode *node = (_CvListNode *) reader.ptr;
1646
1647             prev->next = node;
1648             prev = node;
1649             CV_NEXT_SEQ_ELEM( sizeof( _CvListNode ), reader );
1650         }
1651         prev->next = 0;
1652         prev = stub_node.next;
1653
1654         while( prev )
1655         {
1656             _CvListNode *node = prev->next;
1657             _CvListNode *acc = prev;
1658             _CvPyramidC3 *cmp = (_CvPyramidC3 *) (acc->data);
1659             CvConnectedComp comp;
1660             _CvRGBf c0 = cmp->c;
1661
1662             if( cmp < first_level_end )
1663             {
1664                 icvExpandBaseLevelC3( cmp, &temp_cmp, first_level_start,
1665                                       first_level_size.width );
1666             }
1667             else
1668             {
1669                 temp_cmp = *cmp;
1670                 temp_cmp.c.blue *= temp_cmp.a;
1671                 temp_cmp.c.green *= temp_cmp.a;
1672                 temp_cmp.c.red *= temp_cmp.a;
1673             }
1674
1675             acc->next = 0;
1676             stub_node.next = 0;
1677             prev = &stub_node;
1678
1679             while( node )
1680             {
1681                 cmp = (_CvPyramidC3 *) (node->data);
1682                 if( _CV_RGB_DIST( c0, cmp->c ) < threshold )
1683                 {
1684                     _CvPyramidC3 temp;
1685
1686                     /* exclude from global list and add to list of joint component */
1687                     prev->next = node->next;
1688                     node->next = acc;
1689                     acc = node;
1690
1691                     if( cmp < first_level_end )
1692                     {
1693                         icvExpandBaseLevelC3( cmp, &temp, first_level_start,
1694                                               first_level_size.width );
1695                         cmp = &temp;
1696                     }
1697
1698                     temp_cmp.a += cmp->a;
1699                     temp_cmp.c.blue += cmp->c.blue * cmp->a;
1700                     temp_cmp.c.green += cmp->c.green * cmp->a;
1701                     temp_cmp.c.red += cmp->c.red * cmp->a;
1702                     icvMaxRoi( &(temp_cmp.rect), &(cmp->rect) );
1703                 }
1704                 else
1705                 {
1706                     if( prev == &stub_node )
1707                     {
1708                         stub_node.next = node;
1709                     }
1710                     prev = node;
1711                 }
1712                 node = prev->next;
1713             }
1714
1715             if( temp_cmp.a != 0 )
1716             {
1717                 c_blue = cvRound( temp_cmp.c.blue / temp_cmp.a );
1718                 c_green = cvRound( temp_cmp.c.green / temp_cmp.a );
1719                 c_red = cvRound( temp_cmp.c.red / temp_cmp.a );
1720             }
1721             else
1722             {
1723                 c_blue = cvRound( c0.blue );
1724                 c_green = cvRound( c0.green );
1725                 c_red = cvRound( c0.red );
1726             }
1727             node = acc;
1728
1729             while( node )
1730             {
1731                 Cv32suf _c;
1732                 cmp = (_CvPyramidC3 *) (node->data);
1733                 _c.i = c_blue; cmp->c.blue = _c.f;
1734                 _c.i = c_green; cmp->c.green = _c.f;
1735                 _c.i = c_red; cmp->c.red = _c.f;
1736                 node = node->next;
1737             }
1738
1739             comp.value = cvScalar( c_blue, c_green, c_red );
1740             comp.area = temp_cmp.a;
1741             comp.rect.x = temp_cmp.rect.x1;
1742             comp.rect.y = temp_cmp.rect.y1;
1743             comp.rect.width = temp_cmp.rect.x2 - temp_cmp.rect.x1;
1744             comp.rect.height = temp_cmp.rect.y2 - temp_cmp.rect.y1;
1745             comp.contour = 0;
1746
1747             CV_WRITE_SEQ_ELEM( comp, writer );
1748             prev = stub_node.next;
1749         }
1750     }
1751
1752     cvEndWriteSeq( &writer );
1753     return CV_OK;
1754 }
1755
1756 /****************************************************************************************\
1757
1758                  definition of the maximum roi size
1759
1760 \****************************************************************************************/
1761 void
1762 icvMaxRoi( _CvRect16u * max_rect, _CvRect16u * cur_rect )
1763 {
1764     if( max_rect->x2 == 0 )
1765         *max_rect = *cur_rect;
1766     else
1767     {
1768         if( max_rect->x1 > cur_rect->x1 )
1769             max_rect->x1 = cur_rect->x1;
1770         if( max_rect->y1 > cur_rect->y1 )
1771             max_rect->y1 = cur_rect->y1;
1772
1773         if( max_rect->x2 < cur_rect->x2 )
1774             max_rect->x2 = cur_rect->x2;
1775         if( max_rect->y2 < cur_rect->y2 )
1776             max_rect->y2 = cur_rect->y2;
1777     }
1778 }
1779
1780 void
1781 icvMaxRoi1( _CvRect16u * max_rect, int x, int y )
1782 {
1783     if( max_rect->x2 == 0 )
1784     {
1785         max_rect->x1 = (ushort) x;
1786         max_rect->y1 = (ushort) y;
1787
1788         ++x;
1789         ++y;
1790
1791         max_rect->x2 = (ushort) x;
1792         max_rect->y2 = (ushort) y;
1793     }
1794     else
1795     {
1796         if( max_rect->x1 > x )
1797             max_rect->x1 = (ushort) x;
1798         if( max_rect->y1 > y )
1799             max_rect->y1 = (ushort) y;
1800
1801         ++x;
1802         ++y;
1803
1804         if( max_rect->x2 < x )
1805             max_rect->x2 = (ushort) x;
1806         if( max_rect->y2 < y )
1807             max_rect->y2 = (ushort) y;
1808     }
1809 }
1810
1811
1812 /*F///////////////////////////////////////////////////////////////////////////////////////
1813 //    Name:    cvPyrSegmentation
1814 //    Purpose:
1815 //      segments an image using pyramid-linking technique
1816 //    Context:
1817 //    Parameters:
1818 //      src - source image
1819 //      dst - destination image
1820 //      comp - pointer to returned connected component sequence
1821 //      storage - where the sequence is stored
1822 //      level - maximal pyramid level
1823 //      threshold1 - first threshold, affecting on detalization level when pyramid
1824 //                   is built.
1825 //      threshold2 - second threshold - affects on final components merging.
1826 //    Returns:
1827 //    Notes:
1828 //      Source and destination image must be equal types and channels
1829 //F*/
1830 CV_IMPL void
1831 cvPyrSegmentation( IplImage * src,
1832                    IplImage * dst,
1833                    CvMemStorage * storage,
1834                    CvSeq ** comp, int level, double threshold1, double threshold2 )
1835 {
1836     CvSize src_size, dst_size;
1837     uchar *src_data = 0;
1838     uchar *dst_data = 0;
1839     int src_step = 0, dst_step = 0;
1840     int thresh1 = cvRound( threshold1 );
1841     int thresh2 = cvRound( threshold2 );
1842
1843     CV_FUNCNAME( "cvPyrSegmentation" );
1844
1845     __BEGIN__;
1846
1847     if( src->depth != IPL_DEPTH_8U )
1848         CV_ERROR( CV_BadDepth, cvUnsupportedFormat );
1849
1850     if( src->depth != dst->depth || src->nChannels != dst->nChannels )
1851         CV_ERROR( CV_StsBadArg, "src and dst have different formats" );
1852
1853     cvGetRawData( src, &src_data, &src_step, &src_size );
1854     cvGetRawData( dst, &dst_data, &dst_step, &dst_size );
1855
1856     if( src_size.width != dst_size.width ||
1857         src_size.height != dst_size.height )
1858         CV_ERROR( CV_StsBadArg, "src and dst have different ROIs" );
1859
1860     switch (src->nChannels)
1861     {
1862     case 1:
1863         IPPI_CALL( icvPyrSegmentation8uC1R( src_data, src_step,
1864                                             dst_data, dst_step,
1865                                             src_size,
1866                                             CV_GAUSSIAN_5x5,
1867                                             comp, storage, level, thresh1, thresh2 ));
1868         break;
1869     case 3:
1870         IPPI_CALL( icvPyrSegmentation8uC3R( src_data, src_step,
1871                                             dst_data, dst_step,
1872                                             src_size,
1873                                             CV_GAUSSIAN_5x5,
1874                                             comp, storage, level, thresh1, thresh2 ));
1875         break;
1876     default:
1877         CV_ERROR( CV_BadNumChannels, cvUnsupportedFormat );
1878     }
1879     __END__;
1880 }
1881
1882
1883 /* End of file. */