root/trunk/opencv/src/cv/cvcamshift.cpp @ 32

Revision 32, 9.2 KB (checked in by vp153, 10 years ago)

no message

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