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