camshiftKalman
 All Classes Files Functions Variables Enumerations Enumerator Macros
camShift.h
Go to the documentation of this file.
1 #ifndef CAMSHIFT_H
2 #define CAMSHIFT_H
3 
4 #include <opencv2/highgui/highgui.hpp>
5 #include <opencv/cv.hpp>
6 
7 using namespace cv;
8 
17 int cvmeanShift( const void* imgProb, CvRect windowIn,
18  CvTermCriteria criteria, CvConnectedComp* comp )
19 {
20  CvMoments moments;
21  int i = 0, eps;
22  CvMat stub, *mat = (CvMat*)imgProb;
23  CvMat cur_win;
24  CvRect cur_rect = windowIn;
25 
26  if( comp )
27  comp->rect = windowIn;
28 
29  moments.m00 = moments.m10 = moments.m01 = 0;
30 
31  mat = cvGetMat( mat, &stub );
32 
33  if( CV_MAT_CN( mat->type ) > 1 )
34  CV_Error( CV_BadNumChannels, cvUnsupportedFormat );
35 
36  if( windowIn.height <= 0 || windowIn.width <= 0 )
37  CV_Error( CV_StsBadArg, "Input window has non-positive sizes" );
38 
39  windowIn = cv::Rect(windowIn) & cv::Rect(0, 0, mat->cols, mat->rows);
40 
41  criteria = cvCheckTermCriteria( criteria, 1., 100 );
42  eps = cvRound( criteria.epsilon * criteria.epsilon );
43 
44  for( i = 0; i < criteria.max_iter; i++ )
45  {
46  int dx, dy, nx, ny;
47  double inv_m00;
48  cur_rect = cv::Rect(cur_rect) & cv::Rect(0, 0, mat->cols, mat->rows);
49  if( cv::Rect(cur_rect) == cv::Rect() )
50  {
51  cur_rect.x = mat->cols/2;
52  cur_rect.y = mat->rows/2;
53  }
54  cur_rect.width = MAX(cur_rect.width, 1);
55  cur_rect.height = MAX(cur_rect.height, 1);
56 
57  cvGetSubRect( mat, &cur_win, cur_rect );
58  cvMoments( &cur_win, &moments );
59 
60  /* Calculating center of mass */
61  if( fabs(moments.m00) < DBL_EPSILON )
62  break;
63 
64  inv_m00 = moments.inv_sqrt_m00*moments.inv_sqrt_m00;
65  dx = cvRound( moments.m10 * inv_m00 - windowIn.width*0.5 );
66  dy = cvRound( moments.m01 * inv_m00 - windowIn.height*0.5 );
67 
68  nx = cur_rect.x + dx;
69  ny = cur_rect.y + dy;
70 
71  if( nx < 0 )
72  nx = 0;
73  else if( nx + cur_rect.width > mat->cols )
74  nx = mat->cols - cur_rect.width;
75 
76  if( ny < 0 )
77  ny = 0;
78  else if( ny + cur_rect.height > mat->rows )
79  ny = mat->rows - cur_rect.height;
80 
81  dx = nx - cur_rect.x;
82  dy = ny - cur_rect.y;
83  cur_rect.x = nx;
84  cur_rect.y = ny;
85 
86  /* Check for coverage centers mass & window */
87  if( dx*dx + dy*dy < eps )
88  break;
89  }
90 
91  if( comp )
92  {
93  comp->rect = cur_rect;
94  comp->area = (float)moments.m00;
95  }
96 
97  return i;
98 }
99 
100 
101 /*F///////////////////////////////////////////////////////////////////////////////////////
102 // Name: cvCamShift
103 // Purpose: CAMSHIFT algorithm
104 // Context:
105 // Parameters:
106 // imgProb - 2D object probability distribution
107 // windowIn - CvRect of CAMSHIFT Window intial size
108 // criteria - criteria of stop finding window
109 // windowOut - Location, height and width of converged CAMSHIFT window
110 // orientation - If != NULL, return distribution orientation
111 // len - If != NULL, return equivalent len
112 // width - If != NULL, return equivalent width
113 // area - sum of all elements in result window
114 // Returns:
115 // Number of iterations CAMSHIFT took to converge
116 // Notes:
117 //F*/
125 int cvcamShift( const void* imgProb, CvRect windowIn,
126  CvTermCriteria criteria,
127  CvConnectedComp* _comp,
128  CvBox2D* box )
129 {
130  const int TOLERANCE = 10;
131  CvMoments moments;
132  double m00 = 0, m10, m01, mu20, mu11, mu02, inv_m00;
133  double a, b, c, xc, yc;
134  double rotate_a, rotate_c;
135  double theta = 0, square;
136  double cs, sn;
137  double length = 0, width = 0;
138  int itersUsed = 0;
139  CvConnectedComp comp;
140  CvMat cur_win, stub, *mat = (CvMat*)imgProb;
141 
142  comp.rect = windowIn;
143 
144  mat = cvGetMat( mat, &stub );
145 
146  itersUsed = cvmeanShift( mat, windowIn, criteria, &comp );
147  windowIn = comp.rect;
148 
149  windowIn.x -= TOLERANCE;
150  if( windowIn.x < 0 )
151  windowIn.x = 0;
152 
153  windowIn.y -= TOLERANCE;
154  if( windowIn.y < 0 )
155  windowIn.y = 0;
156 
157  windowIn.width += 2 * TOLERANCE;
158  if( windowIn.x + windowIn.width > mat->width )
159  windowIn.width = mat->width - windowIn.x;
160 
161  windowIn.height += 2 * TOLERANCE;
162  if( windowIn.y + windowIn.height > mat->height )
163  windowIn.height = mat->height - windowIn.y;
164 
165  cvGetSubRect( mat, &cur_win, windowIn );
166 
167  /* Calculating moments in new center mass */
168  cvMoments( &cur_win, &moments );
169 
170  m00 = moments.m00;
171  m10 = moments.m10;
172  m01 = moments.m01;
173  mu11 = moments.mu11;
174  mu20 = moments.mu20;
175  mu02 = moments.mu02;
176 
177  if( fabs(m00) < DBL_EPSILON )
178  return -1;
179 
180  inv_m00 = 1. / m00;
181  xc = cvRound( m10 * inv_m00 + windowIn.x );
182  yc = cvRound( m01 * inv_m00 + windowIn.y );
183  a = mu20 * inv_m00;
184  b = mu11 * inv_m00;
185  c = mu02 * inv_m00;
186 
187  /* Calculating width & height */
188  square = sqrt( 4 * b * b + (a - c) * (a - c) );
189 
190  /* Calculating orientation */
191  theta = atan2( 2 * b, a - c + square );
192 
193  /* Calculating width & length of figure */
194  cs = cos( theta );
195  sn = sin( theta );
196 
197  rotate_a = cs * cs * mu20 + 2 * cs * sn * mu11 + sn * sn * mu02;
198  rotate_c = sn * sn * mu20 - 2 * cs * sn * mu11 + cs * cs * mu02;
199  length = sqrt( rotate_a * inv_m00 ) * 4;
200  width = sqrt( rotate_c * inv_m00 ) * 4;
201 
202  /* In case, when tetta is 0 or 1.57... the Length & Width may be exchanged */
203  if( length < width )
204  {
205  double t;
206 
207  CV_SWAP( length, width, t );
208  CV_SWAP( cs, sn, t );
209  theta = CV_PI*0.5 - theta;
210  }
211 
212  /* Saving results */
213  if( _comp || box )
214  {
215  int t0, t1;
216  int _xc = cvRound( xc );
217  int _yc = cvRound( yc );
218 
219  t0 = cvRound( fabs( length * cs ));
220  t1 = cvRound( fabs( width * sn ));
221 
222  t0 = MAX( t0, t1 ) + 2;
223  comp.rect.width = MIN( t0, (mat->width - _xc) * 2 );
224 
225  t0 = cvRound( fabs( length * sn ));
226  t1 = cvRound( fabs( width * cs ));
227 
228  t0 = MAX( t0, t1 ) + 2;
229  comp.rect.height = MIN( t0, (mat->height - _yc) * 2 );
230 
231  comp.rect.x = MAX( 0, _xc - comp.rect.width / 2 );
232  comp.rect.y = MAX( 0, _yc - comp.rect.height / 2 );
233 
234  comp.rect.width = MIN( mat->width - comp.rect.x, comp.rect.width );
235  comp.rect.height = MIN( mat->height - comp.rect.y, comp.rect.height );
236  comp.area = (float) m00;
237  }
238 
239  if( _comp )
240  *_comp = comp;
241 
242  if( box )
243  {
244  box->size.height = (float)length;
245  box->size.width = (float)width;
246  box->angle = (float)((CV_PI*0.5+theta)*180./CV_PI);
247  while(box->angle < 0)
248  box->angle += 360;
249  while(box->angle >= 360)
250  box->angle -= 360;
251  if(box->angle >= 180)
252  box->angle -= 180;
253  box->center = cvPoint2D32f( comp.rect.x + comp.rect.width*0.5f,
254  comp.rect.y + comp.rect.height*0.5f);
255  }
256 
257  return itersUsed;
258 }
259 
260 RotatedRect camShift( InputArray _probImage, Rect& window,
261  TermCriteria criteria )
262 {
263  CvConnectedComp comp;
264  CvBox2D box;
265 
266  box.center.x = box.center.y = 0; box.angle = 0; box.size.width = box.size.height = 0;
267  comp.rect.x = comp.rect.y = comp.rect.width = comp.rect.height = 0;
268 
269  Mat probImage = _probImage.getMat();
270  CvMat c_probImage = probImage;
271  cvcamShift(&c_probImage, window, (CvTermCriteria)criteria, &comp, &box);
272  window = comp.rect;
273  return RotatedRect(Point2f(box.center), Size2f(box.size), box.angle);
274 }
275 
276 int meanShift( InputArray _probImage, Rect& window, TermCriteria criteria )
277 {
278  CvConnectedComp comp;
279  Mat probImage = _probImage.getMat();
280  CvMat c_probImage = probImage;
281  int iters = cvmeanShift(&c_probImage, window, (CvTermCriteria)criteria, &comp );
282  window = comp.rect;
283  return iters;
284 }
285 
286 #endif // CAMSHIFT_H