git.maemo.org Git - opencv/blob - cv/src/cvcamshift.cpp

   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
  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
 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. */