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*
icvAllocWSNodes(CvMemStorage * storage)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
cvWatershed(const CvArr * srcarr,CvArr * dstarr)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
cvPyrMeanShiftFiltering(const CvArr * srcarr,CvArr * dstarr,double sp0,double sr,int max_level,CvTermCriteria termcrit)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