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 42 #include "_cv.h" 43 44 /****************************************************************************************\ 45 * Watershed * 46 \****************************************************************************************/ 47 48 typedef struct CvWSNode 49 { 50 struct CvWSNode* next; 51 int mask_ofs; 52 int img_ofs; 53 } 54 CvWSNode; 55 56 typedef struct CvWSQueue 57 { 58 CvWSNode* first; 59 CvWSNode* last; 60 } 61 CvWSQueue; 62 63 static CvWSNode* 64 icvAllocWSNodes( CvMemStorage* storage ) 65 { 66 CvWSNode* n = 0; 67 68 CV_FUNCNAME( "icvAllocWSNodes" ); 69 70 __BEGIN__; 71 72 int i, count = (storage->block_size - sizeof(CvMemBlock))/sizeof(*n) - 1; 73 74 CV_CALL( n = (CvWSNode*)cvMemStorageAlloc( storage, count*sizeof(*n) )); 75 for( i = 0; i < count-1; i++ ) 76 n[i].next = n + i + 1; 77 n[count-1].next = 0; 78 79 __END__; 80 81 return n; 82 } 83 84 85 CV_IMPL void 86 cvWatershed( const CvArr* srcarr, CvArr* dstarr ) 87 { 88 const int IN_QUEUE = -2; 89 const int WSHED = -1; 90 const int NQ = 256; 91 CvMemStorage* storage = 0; 92 93 CV_FUNCNAME( "cvWatershed" ); 94 95 __BEGIN__; 96 97 CvMat sstub, *src; 98 CvMat dstub, *dst; 99 CvSize size; 100 CvWSNode* free_node = 0, *node; 101 CvWSQueue q[NQ]; 102 int active_queue; 103 int i, j; 104 int db, dg, dr; 105 int* mask; 106 uchar* img; 107 int mstep, istep; 108 int subs_tab[513]; 109 110 // MAX(a,b) = b + MAX(a-b,0) 111 #define ws_max(a,b) ((b) + subs_tab[(a)-(b)+NQ]) 112 // MIN(a,b) = a - MAX(a-b,0) 113 #define ws_min(a,b) ((a) - subs_tab[(a)-(b)+NQ]) 114 115 #define ws_push(idx,mofs,iofs) \ 116 { \ 117 if( !free_node ) \ 118 CV_CALL( free_node = icvAllocWSNodes( storage ));\ 119 node = free_node; \ 120 free_node = free_node->next;\ 121 node->next = 0; \ 122 node->mask_ofs = mofs; \ 123 node->img_ofs = iofs; \ 124 if( q[idx].last ) \ 125 q[idx].last->next=node; \ 126 else \ 127 q[idx].first = node; \ 128 q[idx].last = node; \ 129 } 130 131 #define ws_pop(idx,mofs,iofs) \ 132 { \ 133 node = q[idx].first; \ 134 q[idx].first = node->next; \ 135 if( !node->next ) \ 136 q[idx].last = 0; \ 137 node->next = free_node; \ 138 free_node = node; \ 139 mofs = node->mask_ofs; \ 140 iofs = node->img_ofs; \ 141 } 142 143 #define c_diff(ptr1,ptr2,diff) \ 144 { \ 145 db = abs((ptr1)[0] - (ptr2)[0]);\ 146 dg = abs((ptr1)[1] - (ptr2)[1]);\ 147 dr = abs((ptr1)[2] - (ptr2)[2]);\ 148 diff = ws_max(db,dg); \ 149 diff = ws_max(diff,dr); \ 150 assert( 0 <= diff && diff <= 255 ); \ 151 } 152 153 CV_CALL( src = cvGetMat( srcarr, &sstub )); 154 CV_CALL( dst = cvGetMat( dstarr, &dstub )); 155 156 if( CV_MAT_TYPE(src->type) != CV_8UC3 ) 157 CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit, 3-channel input images are supported" ); 158 159 if( CV_MAT_TYPE(dst->type) != CV_32SC1 ) 160 CV_ERROR( CV_StsUnsupportedFormat, 161 "Only 32-bit, 1-channel output images are supported" ); 162 163 if( !CV_ARE_SIZES_EQ( src, dst )) 164 CV_ERROR( CV_StsUnmatchedSizes, "The input and output images must have the same size" ); 165 166 size = cvGetMatSize(src); 167 168 CV_CALL( storage = cvCreateMemStorage() ); 169 170 istep = src->step; 171 img = src->data.ptr; 172 mstep = dst->step / sizeof(mask[0]); 173 mask = dst->data.i; 174 175 memset( q, 0, NQ*sizeof(q[0]) ); 176 177 for( i = 0; i < 256; i++ ) 178 subs_tab[i] = 0; 179 for( i = 256; i <= 512; i++ ) 180 subs_tab[i] = i - 256; 181 182 // draw a pixel-wide border of dummy "watershed" (i.e. boundary) pixels 183 for( j = 0; j < size.width; j++ ) 184 mask[j] = mask[j + mstep*(size.height-1)] = WSHED; 185 186 // initial phase: put all the neighbor pixels of each marker to the ordered queue - 187 // determine the initial boundaries of the basins 188 for( i = 1; i < size.height-1; i++ ) 189 { 190 img += istep; mask += mstep; 191 mask[0] = mask[size.width-1] = WSHED; 192 193 for( j = 1; j < size.width-1; j++ ) 194 { 195 int* m = mask + j; 196 if( m[0] < 0 ) m[0] = 0; 197 if( m[0] == 0 && (m[-1] > 0 || m[1] > 0 || m[-mstep] > 0 || m[mstep] > 0) ) 198 { 199 uchar* ptr = img + j*3; 200 int idx = 256, t; 201 if( m[-1] > 0 ) 202 c_diff( ptr, ptr - 3, idx ); 203 if( m[1] > 0 ) 204 { 205 c_diff( ptr, ptr + 3, t ); 206 idx = ws_min( idx, t ); 207 } 208 if( m[-mstep] > 0 ) 209 { 210 c_diff( ptr, ptr - istep, t ); 211 idx = ws_min( idx, t ); 212 } 213 if( m[mstep] > 0 ) 214 { 215 c_diff( ptr, ptr + istep, t ); 216 idx = ws_min( idx, t ); 217 } 218 assert( 0 <= idx && idx <= 255 ); 219 ws_push( idx, i*mstep + j, i*istep + j*3 ); 220 m[0] = IN_QUEUE; 221 } 222 } 223 } 224 225 // find the first non-empty queue 226 for( i = 0; i < NQ; i++ ) 227 if( q[i].first ) 228 break; 229 230 // if there is no markers, exit immediately 231 if( i == NQ ) 232 EXIT; 233 234 active_queue = i; 235 img = src->data.ptr; 236 mask = dst->data.i; 237 238 // recursively fill the basins 239 for(;;) 240 { 241 int mofs, iofs; 242 int lab = 0, t; 243 int* m; 244 uchar* ptr; 245 246 if( q[active_queue].first == 0 ) 247 { 248 for( i = active_queue+1; i < NQ; i++ ) 249 if( q[i].first ) 250 break; 251 if( i == NQ ) 252 break; 253 active_queue = i; 254 } 255 256 ws_pop( active_queue, mofs, iofs ); 257 258 m = mask + mofs; 259 ptr = img + iofs; 260 t = m[-1]; 261 if( t > 0 ) lab = t; 262 t = m[1]; 263 if( t > 0 ) 264 { 265 if( lab == 0 ) lab = t; 266 else if( t != lab ) lab = WSHED; 267 } 268 t = m[-mstep]; 269 if( t > 0 ) 270 { 271 if( lab == 0 ) lab = t; 272 else if( t != lab ) lab = WSHED; 273 } 274 t = m[mstep]; 275 if( t > 0 ) 276 { 277 if( lab == 0 ) lab = t; 278 else if( t != lab ) lab = WSHED; 279 } 280 assert( lab != 0 ); 281 m[0] = lab; 282 if( lab == WSHED ) 283 continue; 284 285 if( m[-1] == 0 ) 286 { 287 c_diff( ptr, ptr - 3, t ); 288 ws_push( t, mofs - 1, iofs - 3 ); 289 active_queue = ws_min( active_queue, t ); 290 m[-1] = IN_QUEUE; 291 } 292 if( m[1] == 0 ) 293 { 294 c_diff( ptr, ptr + 3, t ); 295 ws_push( t, mofs + 1, iofs + 3 ); 296 active_queue = ws_min( active_queue, t ); 297 m[1] = IN_QUEUE; 298 } 299 if( m[-mstep] == 0 ) 300 { 301 c_diff( ptr, ptr - istep, t ); 302 ws_push( t, mofs - mstep, iofs - istep ); 303 active_queue = ws_min( active_queue, t ); 304 m[-mstep] = IN_QUEUE; 305 } 306 if( m[mstep] == 0 ) 307 { 308 c_diff( ptr, ptr + 3, t ); 309 ws_push( t, mofs + mstep, iofs + istep ); 310 active_queue = ws_min( active_queue, t ); 311 m[mstep] = IN_QUEUE; 312 } 313 } 314 315 __END__; 316 317 cvReleaseMemStorage( &storage ); 318 } 319 320 321 /****************************************************************************************\ 322 * Meanshift * 323 \****************************************************************************************/ 324 325 CV_IMPL void 326 cvPyrMeanShiftFiltering( const CvArr* srcarr, CvArr* dstarr, 327 double sp0, double sr, int max_level, 328 CvTermCriteria termcrit ) 329 { 330 const int cn = 3; 331 const int MAX_LEVELS = 8; 332 CvMat* src_pyramid[MAX_LEVELS+1]; 333 CvMat* dst_pyramid[MAX_LEVELS+1]; 334 CvMat* mask0 = 0; 335 int i, j, level; 336 //uchar* submask = 0; 337 338 #define cdiff(ofs0) (tab[c0-dptr[ofs0]+255] + \ 339 tab[c1-dptr[(ofs0)+1]+255] + tab[c2-dptr[(ofs0)+2]+255] >= isr22) 340 341 memset( src_pyramid, 0, sizeof(src_pyramid) ); 342 memset( dst_pyramid, 0, sizeof(dst_pyramid) ); 343 344 CV_FUNCNAME( "cvPyrMeanShiftFiltering" ); 345 346 __BEGIN__; 347 348 double sr2 = sr * sr; 349 int isr2 = cvRound(sr2), isr22 = MAX(isr2,16); 350 int tab[768]; 351 CvMat sstub0, *src0; 352 CvMat dstub0, *dst0; 353 354 CV_CALL( src0 = cvGetMat( srcarr, &sstub0 )); 355 CV_CALL( dst0 = cvGetMat( dstarr, &dstub0 )); 356 357 if( CV_MAT_TYPE(src0->type) != CV_8UC3 ) 358 CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit, 3-channel images are supported" ); 359 360 if( !CV_ARE_TYPES_EQ( src0, dst0 )) 361 CV_ERROR( CV_StsUnmatchedFormats, "The input and output images must have the same type" ); 362 363 if( !CV_ARE_SIZES_EQ( src0, dst0 )) 364 CV_ERROR( CV_StsUnmatchedSizes, "The input and output images must have the same size" ); 365 366 if( (unsigned)max_level > (unsigned)MAX_LEVELS ) 367 CV_ERROR( CV_StsOutOfRange, "The number of pyramid levels is too large or negative" ); 368 369 if( !(termcrit.type & CV_TERMCRIT_ITER) ) 370 termcrit.max_iter = 5; 371 termcrit.max_iter = MAX(termcrit.max_iter,1); 372 termcrit.max_iter = MIN(termcrit.max_iter,100); 373 if( !(termcrit.type & CV_TERMCRIT_EPS) ) 374 termcrit.epsilon = 1.f; 375 termcrit.epsilon = MAX(termcrit.epsilon, 0.f); 376 377 for( i = 0; i < 768; i++ ) 378 tab[i] = (i - 255)*(i - 255); 379 380 // 1. construct pyramid 381 src_pyramid[0] = src0; 382 dst_pyramid[0] = dst0; 383 for( level = 1; level <= max_level; level++ ) 384 { 385 CV_CALL( src_pyramid[level] = cvCreateMat( (src_pyramid[level-1]->rows+1)/2, 386 (src_pyramid[level-1]->cols+1)/2, src_pyramid[level-1]->type )); 387 CV_CALL( dst_pyramid[level] = cvCreateMat( src_pyramid[level]->rows, 388 src_pyramid[level]->cols, src_pyramid[level]->type )); 389 CV_CALL( cvPyrDown( src_pyramid[level-1], src_pyramid[level] )); 390 //CV_CALL( cvResize( src_pyramid[level-1], src_pyramid[level], CV_INTER_AREA )); 391 } 392 393 CV_CALL( mask0 = cvCreateMat( src0->rows, src0->cols, CV_8UC1 )); 394 //CV_CALL( submask = (uchar*)cvAlloc( (sp+2)*(sp+2) )); 395 396 // 2. apply meanshift, starting from the pyramid top (i.e. the smallest layer) 397 for( level = max_level; level >= 0; level-- ) 398 { 399 CvMat* src = src_pyramid[level]; 400 CvSize size = cvGetMatSize(src); 401 uchar* sptr = src->data.ptr; 402 int sstep = src->step; 403 uchar* mask = 0; 404 int mstep = 0; 405 uchar* dptr; 406 int dstep; 407 float sp = (float)(sp0 / (1 << level)); 408 sp = MAX( sp, 1 ); 409 410 if( level < max_level ) 411 { 412 CvSize size1 = cvGetMatSize(dst_pyramid[level+1]); 413 CvMat m = cvMat( size.height, size.width, CV_8UC1, mask0->data.ptr ); 414 dstep = dst_pyramid[level+1]->step; 415 dptr = dst_pyramid[level+1]->data.ptr + dstep + cn; 416 mstep = m.step; 417 mask = m.data.ptr + mstep; 418 //cvResize( dst_pyramid[level+1], dst_pyramid[level], CV_INTER_CUBIC ); 419 cvPyrUp( dst_pyramid[level+1], dst_pyramid[level] ); 420 cvZero( &m ); 421 422 for( i = 1; i < size1.height-1; i++, dptr += dstep - (size1.width-2)*3, mask += mstep*2 ) 423 { 424 for( j = 1; j < size1.width-1; j++, dptr += cn ) 425 { 426 int c0 = dptr[0], c1 = dptr[1], c2 = dptr[2]; 427 mask[j*2 - 1] = cdiff(-3) || cdiff(3) || cdiff(-dstep-3) || cdiff(-dstep) || 428 cdiff(-dstep+3) || cdiff(dstep-3) || cdiff(dstep) || cdiff(dstep+3); 429 } 430 } 431 432 cvDilate( &m, &m, 0, 1 ); 433 mask = m.data.ptr; 434 } 435 436 dptr = dst_pyramid[level]->data.ptr; 437 dstep = dst_pyramid[level]->step; 438 439 for( i = 0; i < size.height; i++, sptr += sstep - size.width*3, 440 dptr += dstep - size.width*3, 441 mask += mstep ) 442 { 443 for( j = 0; j < size.width; j++, sptr += 3, dptr += 3 ) 444 { 445 int x0 = j, y0 = i, x1, y1, iter; 446 int c0, c1, c2; 447 448 if( mask && !mask[j] ) 449 continue; 450 451 c0 = sptr[0], c1 = sptr[1], c2 = sptr[2]; 452 453 // iterate meanshift procedure 454 for( iter = 0; iter < termcrit.max_iter; iter++ ) 455 { 456 uchar* ptr; 457 int x, y, count = 0; 458 int minx, miny, maxx, maxy; 459 int s0 = 0, s1 = 0, s2 = 0, sx = 0, sy = 0; 460 double icount; 461 int stop_flag; 462 463 //mean shift: process pixels in window (p-sigmaSp)x(p+sigmaSp) 464 minx = cvRound(x0 - sp); minx = MAX(minx, 0); 465 miny = cvRound(y0 - sp); miny = MAX(miny, 0); 466 maxx = cvRound(x0 + sp); maxx = MIN(maxx, size.width-1); 467 maxy = cvRound(y0 + sp); maxy = MIN(maxy, size.height-1); 468 ptr = sptr + (miny - i)*sstep + (minx - j)*3; 469 470 for( y = miny; y <= maxy; y++, ptr += sstep - (maxx-minx+1)*3 ) 471 { 472 int row_count = 0; 473 x = minx; 474 for( ; x + 3 <= maxx; x += 4, ptr += 12 ) 475 { 476 int t0 = ptr[0], t1 = ptr[1], t2 = ptr[2]; 477 if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 ) 478 { 479 s0 += t0; s1 += t1; s2 += t2; 480 sx += x; row_count++; 481 } 482 t0 = ptr[3], t1 = ptr[4], t2 = ptr[5]; 483 if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 ) 484 { 485 s0 += t0; s1 += t1; s2 += t2; 486 sx += x+1; row_count++; 487 } 488 t0 = ptr[6], t1 = ptr[7], t2 = ptr[8]; 489 if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 ) 490 { 491 s0 += t0; s1 += t1; s2 += t2; 492 sx += x+2; row_count++; 493 } 494 t0 = ptr[9], t1 = ptr[10], t2 = ptr[11]; 495 if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 ) 496 { 497 s0 += t0; s1 += t1; s2 += t2; 498 sx += x+3; row_count++; 499 } 500 } 501 502 for( ; x <= maxx; x++, ptr += 3 ) 503 { 504 int t0 = ptr[0], t1 = ptr[1], t2 = ptr[2]; 505 if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 ) 506 { 507 s0 += t0; s1 += t1; s2 += t2; 508 sx += x; row_count++; 509 } 510 } 511 count += row_count; 512 sy += y*row_count; 513 } 514 515 if( count == 0 ) 516 break; 517 518 icount = 1./count; 519 x1 = cvRound(sx*icount); 520 y1 = cvRound(sy*icount); 521 s0 = cvRound(s0*icount); 522 s1 = cvRound(s1*icount); 523 s2 = cvRound(s2*icount); 524 525 stop_flag = (x0 == x1 && y0 == y1) || abs(x1-x0) + abs(y1-y0) + 526 tab[s0 - c0 + 255] + tab[s1 - c1 + 255] + 527 tab[s2 - c2 + 255] <= termcrit.epsilon; 528 529 x0 = x1; y0 = y1; 530 c0 = s0; c1 = s1; c2 = s2; 531 532 if( stop_flag ) 533 break; 534 } 535 536 dptr[0] = (uchar)c0; 537 dptr[1] = (uchar)c1; 538 dptr[2] = (uchar)c2; 539 } 540 } 541 } 542 543 __END__; 544 545 for( i = 1; i <= MAX_LEVELS; i++ ) 546 { 547 cvReleaseMat( &src_pyramid[i] ); 548 cvReleaseMat( &dst_pyramid[i] ); 549 } 550 cvReleaseMat( &mask0 ); 551 } 552 553
