1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
22 //
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
26 //
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 #include "precomp.hpp"
44
45 /****************************************************************************************\
46 * Watershed *
47 \****************************************************************************************/
48
49 namespace cv
50 {
51 // A node represents a pixel to label
52 struct WSNode
53 {
54 int next;
55 int mask_ofs;
56 int img_ofs;
57 };
58
59 // Queue for WSNodes
60 struct WSQueue
61 {
WSQueuecv::WSQueue62 WSQueue() { first = last = 0; }
63 int first, last;
64 };
65
66
67 static int
allocWSNodes(std::vector<WSNode> & storage)68 allocWSNodes( std::vector<WSNode>& storage )
69 {
70 int sz = (int)storage.size();
71 int newsz = MAX(128, sz*3/2);
72
73 storage.resize(newsz);
74 if( sz == 0 )
75 {
76 storage[0].next = 0;
77 sz = 1;
78 }
79 for( int i = sz; i < newsz-1; i++ )
80 storage[i].next = i+1;
81 storage[newsz-1].next = 0;
82 return sz;
83 }
84
85 }
86
87
watershed(InputArray _src,InputOutputArray _markers)88 void cv::watershed( InputArray _src, InputOutputArray _markers )
89 {
90 CV_INSTRUMENT_REGION();
91
92 // Labels for pixels
93 const int IN_QUEUE = -2; // Pixel visited
94 const int WSHED = -1; // Pixel belongs to watershed
95
96 // possible bit values = 2^8
97 const int NQ = 256;
98
99 Mat src = _src.getMat(), dst = _markers.getMat();
100 Size size = src.size();
101
102 // Vector of every created node
103 std::vector<WSNode> storage;
104 int free_node = 0, node;
105 // Priority queue of queues of nodes
106 // from high priority (0) to low priority (255)
107 WSQueue q[NQ];
108 // Non-empty queue with highest priority
109 int active_queue;
110 int i, j;
111 // Color differences
112 int db, dg, dr;
113 int subs_tab[513];
114
115 // MAX(a,b) = b + MAX(a-b,0)
116 #define ws_max(a,b) ((b) + subs_tab[(a)-(b)+NQ])
117 // MIN(a,b) = a - MAX(a-b,0)
118 #define ws_min(a,b) ((a) - subs_tab[(a)-(b)+NQ])
119
120 // Create a new node with offsets mofs and iofs in queue idx
121 #define ws_push(idx,mofs,iofs) \
122 { \
123 if( !free_node ) \
124 free_node = allocWSNodes( storage );\
125 node = free_node; \
126 free_node = storage[free_node].next;\
127 storage[node].next = 0; \
128 storage[node].mask_ofs = mofs; \
129 storage[node].img_ofs = iofs; \
130 if( q[idx].last ) \
131 storage[q[idx].last].next=node; \
132 else \
133 q[idx].first = node; \
134 q[idx].last = node; \
135 }
136
137 // Get next node from queue idx
138 #define ws_pop(idx,mofs,iofs) \
139 { \
140 node = q[idx].first; \
141 q[idx].first = storage[node].next; \
142 if( !storage[node].next ) \
143 q[idx].last = 0; \
144 storage[node].next = free_node; \
145 free_node = node; \
146 mofs = storage[node].mask_ofs; \
147 iofs = storage[node].img_ofs; \
148 }
149
150 // Get highest absolute channel difference in diff
151 #define c_diff(ptr1,ptr2,diff) \
152 { \
153 db = std::abs((ptr1)[0] - (ptr2)[0]);\
154 dg = std::abs((ptr1)[1] - (ptr2)[1]);\
155 dr = std::abs((ptr1)[2] - (ptr2)[2]);\
156 diff = ws_max(db,dg); \
157 diff = ws_max(diff,dr); \
158 assert( 0 <= diff && diff <= 255 ); \
159 }
160
161 CV_Assert( src.type() == CV_8UC3 && dst.type() == CV_32SC1 );
162 CV_Assert( src.size() == dst.size() );
163
164 // Current pixel in input image
165 const uchar* img = src.ptr();
166 // Step size to next row in input image
167 int istep = int(src.step/sizeof(img[0]));
168
169 // Current pixel in mask image
170 int* mask = dst.ptr<int>();
171 // Step size to next row in mask image
172 int mstep = int(dst.step / sizeof(mask[0]));
173
174 for( i = 0; i < 256; i++ )
175 subs_tab[i] = 0;
176 for( i = 256; i <= 512; i++ )
177 subs_tab[i] = i - 256;
178
179 // draw a pixel-wide border of dummy "watershed" (i.e. boundary) pixels
180 for( j = 0; j < size.width; j++ )
181 mask[j] = mask[j + mstep*(size.height-1)] = WSHED;
182
183 // initial phase: put all the neighbor pixels of each marker to the ordered queue -
184 // determine the initial boundaries of the basins
185 for( i = 1; i < size.height-1; i++ )
186 {
187 img += istep; mask += mstep;
188 mask[0] = mask[size.width-1] = WSHED; // boundary pixels
189
190 for( j = 1; j < size.width-1; j++ )
191 {
192 int* m = mask + j;
193 if( m[0] < 0 ) m[0] = 0;
194 if( m[0] == 0 && (m[-1] > 0 || m[1] > 0 || m[-mstep] > 0 || m[mstep] > 0) )
195 {
196 // Find smallest difference to adjacent markers
197 const uchar* ptr = img + j*3;
198 int idx = 256, t;
199 if( m[-1] > 0 )
200 c_diff( ptr, ptr - 3, idx );
201 if( m[1] > 0 )
202 {
203 c_diff( ptr, ptr + 3, t );
204 idx = ws_min( idx, t );
205 }
206 if( m[-mstep] > 0 )
207 {
208 c_diff( ptr, ptr - istep, t );
209 idx = ws_min( idx, t );
210 }
211 if( m[mstep] > 0 )
212 {
213 c_diff( ptr, ptr + istep, t );
214 idx = ws_min( idx, t );
215 }
216
217 // Add to according queue
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 return;
233
234 active_queue = i;
235 img = src.ptr();
236 mask = dst.ptr<int>();
237
238 // recursively fill the basins
239 for(;;)
240 {
241 int mofs, iofs;
242 int lab = 0, t;
243 int* m;
244 const uchar* ptr;
245
246 // Get non-empty queue with highest priority
247 // Exit condition: empty priority queue
248 if( q[active_queue].first == 0 )
249 {
250 for( i = active_queue+1; i < NQ; i++ )
251 if( q[i].first )
252 break;
253 if( i == NQ )
254 break;
255 active_queue = i;
256 }
257
258 // Get next node
259 ws_pop( active_queue, mofs, iofs );
260
261 // Calculate pointer to current pixel in input and marker image
262 m = mask + mofs;
263 ptr = img + iofs;
264
265 // Check surrounding pixels for labels
266 // to determine label for current pixel
267 t = m[-1]; // Left
268 if( t > 0 ) lab = t;
269 t = m[1]; // Right
270 if( t > 0 )
271 {
272 if( lab == 0 ) lab = t;
273 else if( t != lab ) lab = WSHED;
274 }
275 t = m[-mstep]; // Top
276 if( t > 0 )
277 {
278 if( lab == 0 ) lab = t;
279 else if( t != lab ) lab = WSHED;
280 }
281 t = m[mstep]; // Bottom
282 if( t > 0 )
283 {
284 if( lab == 0 ) lab = t;
285 else if( t != lab ) lab = WSHED;
286 }
287
288 // Set label to current pixel in marker image
289 assert( lab != 0 );
290 m[0] = lab;
291
292 if( lab == WSHED )
293 continue;
294
295 // Add adjacent, unlabeled pixels to corresponding queue
296 if( m[-1] == 0 )
297 {
298 c_diff( ptr, ptr - 3, t );
299 ws_push( t, mofs - 1, iofs - 3 );
300 active_queue = ws_min( active_queue, t );
301 m[-1] = IN_QUEUE;
302 }
303 if( m[1] == 0 )
304 {
305 c_diff( ptr, ptr + 3, t );
306 ws_push( t, mofs + 1, iofs + 3 );
307 active_queue = ws_min( active_queue, t );
308 m[1] = IN_QUEUE;
309 }
310 if( m[-mstep] == 0 )
311 {
312 c_diff( ptr, ptr - istep, t );
313 ws_push( t, mofs - mstep, iofs - istep );
314 active_queue = ws_min( active_queue, t );
315 m[-mstep] = IN_QUEUE;
316 }
317 if( m[mstep] == 0 )
318 {
319 c_diff( ptr, ptr + istep, t );
320 ws_push( t, mofs + mstep, iofs + istep );
321 active_queue = ws_min( active_queue, t );
322 m[mstep] = IN_QUEUE;
323 }
324 }
325 }
326
327
328 /****************************************************************************************\
329 * Meanshift *
330 \****************************************************************************************/
331
332
pyrMeanShiftFiltering(InputArray _src,OutputArray _dst,double sp0,double sr,int max_level,TermCriteria termcrit)333 void cv::pyrMeanShiftFiltering( InputArray _src, OutputArray _dst,
334 double sp0, double sr, int max_level,
335 TermCriteria termcrit )
336 {
337 CV_INSTRUMENT_REGION();
338
339 Mat src0 = _src.getMat();
340
341 if( src0.empty() )
342 return;
343
344 _dst.create( src0.size(), src0.type() );
345 Mat dst0 = _dst.getMat();
346
347 const int cn = 3;
348 const int MAX_LEVELS = 8;
349
350 if( (unsigned)max_level > (unsigned)MAX_LEVELS )
351 CV_Error( CV_StsOutOfRange, "The number of pyramid levels is too large or negative" );
352
353 std::vector<cv::Mat> src_pyramid(max_level+1);
354 std::vector<cv::Mat> dst_pyramid(max_level+1);
355 cv::Mat mask0;
356 int i, j, level;
357 //uchar* submask = 0;
358
359 #define cdiff(ofs0) (tab[c0-dptr[ofs0]+255] + \
360 tab[c1-dptr[(ofs0)+1]+255] + tab[c2-dptr[(ofs0)+2]+255] >= isr22)
361
362 double sr2 = sr * sr;
363 int isr2 = cvRound(sr2), isr22 = MAX(isr2,16);
364 int tab[768];
365
366
367 if( src0.type() != CV_8UC3 )
368 CV_Error( CV_StsUnsupportedFormat, "Only 8-bit, 3-channel images are supported" );
369
370 if( src0.type() != dst0.type() )
371 CV_Error( CV_StsUnmatchedFormats, "The input and output images must have the same type" );
372
373 if( src0.size() != dst0.size() )
374 CV_Error( CV_StsUnmatchedSizes, "The input and output images must have the same size" );
375
376 if( !(termcrit.type & CV_TERMCRIT_ITER) )
377 termcrit.maxCount = 5;
378 termcrit.maxCount = MAX(termcrit.maxCount,1);
379 termcrit.maxCount = MIN(termcrit.maxCount,100);
380 if( !(termcrit.type & CV_TERMCRIT_EPS) )
381 termcrit.epsilon = 1.f;
382 termcrit.epsilon = MAX(termcrit.epsilon, 0.f);
383
384 for( i = 0; i < 768; i++ )
385 tab[i] = (i - 255)*(i - 255);
386
387 // 1. construct pyramid
388 src_pyramid[0] = src0;
389 dst_pyramid[0] = dst0;
390 for( level = 1; level <= max_level; level++ )
391 {
392 src_pyramid[level].create( (src_pyramid[level-1].rows+1)/2,
393 (src_pyramid[level-1].cols+1)/2, src_pyramid[level-1].type() );
394 dst_pyramid[level].create( src_pyramid[level].rows,
395 src_pyramid[level].cols, src_pyramid[level].type() );
396 cv::pyrDown( src_pyramid[level-1], src_pyramid[level], src_pyramid[level].size() );
397 //CV_CALL( cvResize( src_pyramid[level-1], src_pyramid[level], CV_INTER_AREA ));
398 }
399
400 mask0.create(src0.rows, src0.cols, CV_8UC1);
401 //CV_CALL( submask = (uchar*)cvAlloc( (sp+2)*(sp+2) ));
402
403 // 2. apply meanshift, starting from the pyramid top (i.e. the smallest layer)
404 for( level = max_level; level >= 0; level-- )
405 {
406 cv::Mat src = src_pyramid[level];
407 cv::Size size = src.size();
408 const uchar* sptr = src.ptr();
409 int sstep = (int)src.step;
410 uchar* dptr;
411 int dstep;
412 float sp = (float)(sp0 / (1 << level));
413 sp = MAX( sp, 1 );
414
415 cv::Mat m;
416 if( level < max_level )
417 {
418 cv::Size size1 = dst_pyramid[level+1].size();
419 m = cv::Mat(size.height, size.width, CV_8UC1, mask0.ptr());
420 dstep = (int)dst_pyramid[level+1].step;
421 dptr = dst_pyramid[level+1].ptr() + dstep + cn;
422 //cvResize( dst_pyramid[level+1], dst_pyramid[level], CV_INTER_CUBIC );
423 cv::pyrUp( dst_pyramid[level+1], dst_pyramid[level], dst_pyramid[level].size() );
424 m.setTo(cv::Scalar::all(0));
425
426 for( i = 1; i < size1.height-1; i++, dptr += dstep - (size1.width-2)*3)
427 {
428 uchar* mask = m.ptr(1 + i * 2);
429 for( j = 1; j < size1.width-1; j++, dptr += cn )
430 {
431 int c0 = dptr[0], c1 = dptr[1], c2 = dptr[2];
432 mask[j*2 - 1] = cdiff(-3) || cdiff(3) || cdiff(-dstep-3) || cdiff(-dstep) ||
433 cdiff(-dstep+3) || cdiff(dstep-3) || cdiff(dstep) || cdiff(dstep+3);
434 }
435 }
436
437 cv::dilate( m, m, cv::Mat() );
438 }
439
440 dptr = dst_pyramid[level].ptr();
441 dstep = (int)dst_pyramid[level].step;
442
443 for( i = 0; i < size.height; i++, sptr += sstep - size.width*3,
444 dptr += dstep - size.width*3
445 )
446 {
447 uchar* mask = m.empty() ? NULL : m.ptr(i);
448 for( j = 0; j < size.width; j++, sptr += 3, dptr += 3 )
449 {
450 int x0 = j, y0 = i, x1, y1, iter;
451 int c0, c1, c2;
452
453 if( mask && !mask[j] )
454 continue;
455
456 c0 = sptr[0], c1 = sptr[1], c2 = sptr[2];
457
458 // iterate meanshift procedure
459 for( iter = 0; iter < termcrit.maxCount; iter++ )
460 {
461 const uchar* ptr;
462 int x, y, count = 0;
463 int minx, miny, maxx, maxy;
464 int s0 = 0, s1 = 0, s2 = 0, sx = 0, sy = 0;
465 double icount;
466 int stop_flag;
467
468 //mean shift: process pixels in window (p-sigmaSp)x(p+sigmaSp)
469 minx = cvRound(x0 - sp); minx = MAX(minx, 0);
470 miny = cvRound(y0 - sp); miny = MAX(miny, 0);
471 maxx = cvRound(x0 + sp); maxx = MIN(maxx, size.width-1);
472 maxy = cvRound(y0 + sp); maxy = MIN(maxy, size.height-1);
473 ptr = sptr + (miny - i)*sstep + (minx - j)*3;
474
475 for( y = miny; y <= maxy; y++, ptr += sstep - (maxx-minx+1)*3 )
476 {
477 int row_count = 0;
478 x = minx;
479 #if CV_ENABLE_UNROLLED
480 for( ; x + 3 <= maxx; x += 4, ptr += 12 )
481 {
482 int t0 = ptr[0], t1 = ptr[1], t2 = ptr[2];
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; row_count++;
487 }
488 t0 = ptr[3], t1 = ptr[4], t2 = ptr[5];
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+1; row_count++;
493 }
494 t0 = ptr[6], t1 = ptr[7], t2 = ptr[8];
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+2; row_count++;
499 }
500 t0 = ptr[9], t1 = ptr[10], t2 = ptr[11];
501 if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 )
502 {
503 s0 += t0; s1 += t1; s2 += t2;
504 sx += x+3; row_count++;
505 }
506 }
507 #endif
508 for( ; x <= maxx; x++, ptr += 3 )
509 {
510 int t0 = ptr[0], t1 = ptr[1], t2 = ptr[2];
511 if( tab[t0-c0+255] + tab[t1-c1+255] + tab[t2-c2+255] <= isr2 )
512 {
513 s0 += t0; s1 += t1; s2 += t2;
514 sx += x; row_count++;
515 }
516 }
517 count += row_count;
518 sy += y*row_count;
519 }
520
521 if( count == 0 )
522 break;
523
524 icount = 1./count;
525 x1 = cvRound(sx*icount);
526 y1 = cvRound(sy*icount);
527 s0 = cvRound(s0*icount);
528 s1 = cvRound(s1*icount);
529 s2 = cvRound(s2*icount);
530
531 stop_flag = (x0 == x1 && y0 == y1) || std::abs(x1-x0) + std::abs(y1-y0) +
532 tab[s0 - c0 + 255] + tab[s1 - c1 + 255] +
533 tab[s2 - c2 + 255] <= termcrit.epsilon;
534
535 x0 = x1; y0 = y1;
536 c0 = s0; c1 = s1; c2 = s2;
537
538 if( stop_flag )
539 break;
540 }
541
542 dptr[0] = (uchar)c0;
543 dptr[1] = (uchar)c1;
544 dptr[2] = (uchar)c2;
545 }
546 }
547 }
548 }
549
550
551 ///////////////////////////////////////////////////////////////////////////////////////////////
552
cvWatershed(const CvArr * _src,CvArr * _markers)553 CV_IMPL void cvWatershed( const CvArr* _src, CvArr* _markers )
554 {
555 cv::Mat src = cv::cvarrToMat(_src), markers = cv::cvarrToMat(_markers);
556 cv::watershed(src, markers);
557 }
558
559
560 CV_IMPL void
cvPyrMeanShiftFiltering(const CvArr * srcarr,CvArr * dstarr,double sp0,double sr,int max_level,CvTermCriteria termcrit)561 cvPyrMeanShiftFiltering( const CvArr* srcarr, CvArr* dstarr,
562 double sp0, double sr, int max_level,
563 CvTermCriteria termcrit )
564 {
565 cv::Mat src = cv::cvarrToMat(srcarr);
566 const cv::Mat dst = cv::cvarrToMat(dstarr);
567
568 cv::pyrMeanShiftFiltering(src, dst, sp0, sr, max_level, termcrit);
569 }
570