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
44 /*F///////////////////////////////////////////////////////////////////////////////////////
45 // Name: cvMeanShift
46 // Purpose: MeanShift algorithm
47 // Context:
48 // Parameters:
49 // imgProb - 2D object probability distribution
50 // windowIn - CvRect of CAMSHIFT Window intial size
51 // numIters - If CAMSHIFT iterates this many times, stop
52 // windowOut - Location, height and width of converged CAMSHIFT window
53 // len - If != NULL, return equivalent len
54 // width - If != NULL, return equivalent width
55 // itersUsed - Returns number of iterations CAMSHIFT took to converge
56 // Returns:
57 // The function itself returns the area found
58 // Notes:
59 //F*/
60 CV_IMPL int
cvMeanShift(const void * imgProb,CvRect windowIn,CvTermCriteria criteria,CvConnectedComp * comp)61 cvMeanShift( const void* imgProb, CvRect windowIn,
62 CvTermCriteria criteria, CvConnectedComp* comp )
63 {
64 CvMoments moments;
65 int i = 0, eps;
66 CvMat stub, *mat = (CvMat*)imgProb;
67 CvMat cur_win;
68 CvRect cur_rect = windowIn;
69
70 CV_FUNCNAME( "cvMeanShift" );
71
72 if( comp )
73 comp->rect = windowIn;
74
75 moments.m00 = moments.m10 = moments.m01 = 0;
76
77 __BEGIN__;
78
79 CV_CALL( mat = cvGetMat( mat, &stub ));
80
81 if( CV_MAT_CN( mat->type ) > 1 )
82 CV_ERROR( CV_BadNumChannels, cvUnsupportedFormat );
83
84 if( windowIn.height <= 0 || windowIn.width <= 0 )
85 CV_ERROR( CV_StsBadArg, "Input window has non-positive sizes" );
86
87 if( windowIn.x < 0 || windowIn.x + windowIn.width > mat->cols ||
88 windowIn.y < 0 || windowIn.y + windowIn.height > mat->rows )
89 CV_ERROR( CV_StsBadArg, "Initial window is not inside the image ROI" );
90
91 CV_CALL( criteria = cvCheckTermCriteria( criteria, 1., 100 ));
92
93 eps = cvRound( criteria.epsilon * criteria.epsilon );
94
95 for( i = 0; i < criteria.max_iter; i++ )
96 {
97 int dx, dy, nx, ny;
98 double inv_m00;
99
100 CV_CALL( cvGetSubRect( mat, &cur_win, cur_rect ));
101 CV_CALL( cvMoments( &cur_win, &moments ));
102
103 /* Calculating center of mass */
104 if( fabs(moments.m00) < DBL_EPSILON )
105 break;
106
107 inv_m00 = moments.inv_sqrt_m00*moments.inv_sqrt_m00;
108 dx = cvRound( moments.m10 * inv_m00 - windowIn.width*0.5 );
109 dy = cvRound( moments.m01 * inv_m00 - windowIn.height*0.5 );
110
111 nx = cur_rect.x + dx;
112 ny = cur_rect.y + dy;
113
114 if( nx < 0 )
115 nx = 0;
116 else if( nx + cur_rect.width > mat->cols )
117 nx = mat->cols - cur_rect.width;
118
119 if( ny < 0 )
120 ny = 0;
121 else if( ny + cur_rect.height > mat->rows )
122 ny = mat->rows - cur_rect.height;
123
124 dx = nx - cur_rect.x;
125 dy = ny - cur_rect.y;
126 cur_rect.x = nx;
127 cur_rect.y = ny;
128
129 /* Check for coverage centers mass & window */
130 if( dx*dx + dy*dy < eps )
131 break;
132 }
133
134 __END__;
135
136 if( comp )
137 {
138 comp->rect = cur_rect;
139 comp->area = (float)moments.m00;
140 }
141
142 return i;
143 }
144
145
146 /*F///////////////////////////////////////////////////////////////////////////////////////
147 // Name: cvCamShift
148 // Purpose: CAMSHIFT algorithm
149 // Context:
150 // Parameters:
151 // imgProb - 2D object probability distribution
152 // windowIn - CvRect of CAMSHIFT Window intial size
153 // criteria - criteria of stop finding window
154 // windowOut - Location, height and width of converged CAMSHIFT window
155 // orientation - If != NULL, return distribution orientation
156 // len - If != NULL, return equivalent len
157 // width - If != NULL, return equivalent width
158 // area - sum of all elements in result window
159 // itersUsed - Returns number of iterations CAMSHIFT took to converge
160 // Returns:
161 // The function itself returns the area found
162 // Notes:
163 //F*/
164 CV_IMPL int
cvCamShift(const void * imgProb,CvRect windowIn,CvTermCriteria criteria,CvConnectedComp * _comp,CvBox2D * box)165 cvCamShift( const void* imgProb, CvRect windowIn,
166 CvTermCriteria criteria,
167 CvConnectedComp* _comp,
168 CvBox2D* box )
169 {
170 const int TOLERANCE = 10;
171 CvMoments moments;
172 double m00 = 0, m10, m01, mu20, mu11, mu02, inv_m00;
173 double a, b, c, xc, yc;
174 double rotate_a, rotate_c;
175 double theta = 0, square;
176 double cs, sn;
177 double length = 0, width = 0;
178 int itersUsed = 0;
179 CvConnectedComp comp;
180 CvMat cur_win, stub, *mat = (CvMat*)imgProb;
181
182 CV_FUNCNAME( "cvCamShift" );
183
184 comp.rect = windowIn;
185
186 __BEGIN__;
187
188 CV_CALL( mat = cvGetMat( mat, &stub ));
189
190 CV_CALL( itersUsed = cvMeanShift( mat, windowIn, criteria, &comp ));
191 windowIn = comp.rect;
192
193 windowIn.x -= TOLERANCE;
194 if( windowIn.x < 0 )
195 windowIn.x = 0;
196
197 windowIn.y -= TOLERANCE;
198 if( windowIn.y < 0 )
199 windowIn.y = 0;
200
201 windowIn.width += 2 * TOLERANCE;
202 if( windowIn.x + windowIn.width > mat->width )
203 windowIn.width = mat->width - windowIn.x;
204
205 windowIn.height += 2 * TOLERANCE;
206 if( windowIn.y + windowIn.height > mat->height )
207 windowIn.height = mat->height - windowIn.y;
208
209 CV_CALL( cvGetSubRect( mat, &cur_win, windowIn ));
210
211 /* Calculating moments in new center mass */
212 CV_CALL( cvMoments( &cur_win, &moments ));
213
214 m00 = moments.m00;
215 m10 = moments.m10;
216 m01 = moments.m01;
217 mu11 = moments.mu11;
218 mu20 = moments.mu20;
219 mu02 = moments.mu02;
220
221 if( fabs(m00) < DBL_EPSILON )
222 EXIT;
223
224 inv_m00 = 1. / m00;
225 xc = cvRound( m10 * inv_m00 + windowIn.x );
226 yc = cvRound( m01 * inv_m00 + windowIn.y );
227 a = mu20 * inv_m00;
228 b = mu11 * inv_m00;
229 c = mu02 * inv_m00;
230
231 /* Calculating width & height */
232 square = sqrt( 4 * b * b + (a - c) * (a - c) );
233
234 /* Calculating orientation */
235 theta = atan2( 2 * b, a - c + square );
236
237 /* Calculating width & length of figure */
238 cs = cos( theta );
239 sn = sin( theta );
240
241 rotate_a = cs * cs * mu20 + 2 * cs * sn * mu11 + sn * sn * mu02;
242 rotate_c = sn * sn * mu20 - 2 * cs * sn * mu11 + cs * cs * mu02;
243 length = sqrt( rotate_a * inv_m00 ) * 4;
244 width = sqrt( rotate_c * inv_m00 ) * 4;
245
246 /* In case, when tetta is 0 or 1.57... the Length & Width may be exchanged */
247 if( length < width )
248 {
249 double t;
250
251 CV_SWAP( length, width, t );
252 CV_SWAP( cs, sn, t );
253 theta = CV_PI*0.5 - theta;
254 }
255
256 /* Saving results */
257 if( _comp || box )
258 {
259 int t0, t1;
260 int _xc = cvRound( xc );
261 int _yc = cvRound( yc );
262
263 t0 = cvRound( fabs( length * cs ));
264 t1 = cvRound( fabs( width * sn ));
265
266 t0 = MAX( t0, t1 ) + 2;
267 comp.rect.width = MIN( t0, (mat->width - _xc) * 2 );
268
269 t0 = cvRound( fabs( length * sn ));
270 t1 = cvRound( fabs( width * cs ));
271
272 t0 = MAX( t0, t1 ) + 2;
273 comp.rect.height = MIN( t0, (mat->height - _yc) * 2 );
274
275 comp.rect.x = MAX( 0, _xc - comp.rect.width / 2 );
276 comp.rect.y = MAX( 0, _yc - comp.rect.height / 2 );
277
278 comp.rect.width = MIN( mat->width - comp.rect.x, comp.rect.width );
279 comp.rect.height = MIN( mat->height - comp.rect.y, comp.rect.height );
280 comp.area = (float) m00;
281 }
282
283 __END__;
284
285 if( _comp )
286 *_comp = comp;
287
288 if( box )
289 {
290 box->size.height = (float)length;
291 box->size.width = (float)width;
292 box->angle = (float)(theta*180./CV_PI);
293 box->center = cvPoint2D32f( comp.rect.x + comp.rect.width*0.5f,
294 comp.rect.y + comp.rect.height*0.5f);
295 }
296
297 return itersUsed;
298 }
299
300 /* End of file. */
301