← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/modules/video/src/camshift.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/modules/video/src/camshift.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
//                           License Agreement
 
11
//                For Open Source Computer Vision Library
 
12
//
 
13
// Copyright (C) 2000, Intel Corporation, all rights reserved.
 
14
// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
 
15
// Third party copyrights are property of their respective owners.
 
16
//
 
17
// Redistribution and use in source and binary forms, with or without modification,
 
18
// are permitted provided that the following conditions are met:
 
19
//
 
20
//   * Redistribution's of source code must retain the above copyright notice,
 
21
//     this list of conditions and the following disclaimer.
 
22
//
 
23
//   * Redistribution's in binary form must reproduce the above copyright notice,
 
24
//     this list of conditions and the following disclaimer in the documentation
 
25
//     and/or other materials provided with the distribution.
 
26
//
 
27
//   * The name of Intel Corporation may not be used to endorse or promote products
 
28
//     derived from this software without specific prior written permission.
 
29
//
 
30
// This software is provided by the copyright holders and contributors "as is" and
 
31
// any express or implied warranties, including, but not limited to, the implied
 
32
// warranties of merchantability and fitness for a particular purpose are disclaimed.
 
33
// In no event shall the Intel Corporation or contributors be liable for any direct,
 
34
// indirect, incidental, special, exemplary, or consequential damages
 
35
// (including, but not limited to, procurement of substitute goods or services;
 
36
// loss of use, data, or profits; or business interruption) however caused
 
37
// and on any theory of liability, whether in contract, strict liability,
 
38
// or tort (including negligence or otherwise) arising in any way out of
 
39
// the use of this software, even if advised of the possibility of such damage.
 
40
//
 
41
//M*/
 
42
#include "precomp.hpp"
 
43
 
 
44
int cv::meanShift( InputArray _probImage, Rect& window, TermCriteria criteria )
 
45
{
 
46
    Size size;
 
47
    int cn;
 
48
    Mat mat;
 
49
    UMat umat;
 
50
    bool isUMat = _probImage.isUMat();
 
51
 
 
52
    if (isUMat)
 
53
        umat = _probImage.getUMat(), cn = umat.channels(), size = umat.size();
 
54
    else
 
55
        mat = _probImage.getMat(), cn = mat.channels(), size = mat.size();
 
56
 
 
57
    Rect cur_rect = window;
 
58
 
 
59
    CV_Assert( cn == 1 );
 
60
 
 
61
    if( window.height <= 0 || window.width <= 0 )
 
62
        CV_Error( Error::StsBadArg, "Input window has non-positive sizes" );
 
63
 
 
64
    window = window & Rect(0, 0, size.width, size.height);
 
65
 
 
66
    double eps = (criteria.type & TermCriteria::EPS) ? std::max(criteria.epsilon, 0.) : 1.;
 
67
    eps = cvRound(eps*eps);
 
68
    int i, niters = (criteria.type & TermCriteria::MAX_ITER) ? std::max(criteria.maxCount, 1) : 100;
 
69
 
 
70
    for( i = 0; i < niters; i++ )
 
71
    {
 
72
        cur_rect = cur_rect & Rect(0, 0, size.width, size.height);
 
73
        if( cur_rect == Rect() )
 
74
        {
 
75
            cur_rect.x = size.width/2;
 
76
            cur_rect.y = size.height/2;
 
77
        }
 
78
        cur_rect.width = std::max(cur_rect.width, 1);
 
79
        cur_rect.height = std::max(cur_rect.height, 1);
 
80
 
 
81
        Moments m = isUMat ? moments(umat(cur_rect)) : moments(mat(cur_rect));
 
82
 
 
83
        // Calculating center of mass
 
84
        if( fabs(m.m00) < DBL_EPSILON )
 
85
            break;
 
86
 
 
87
        int dx = cvRound( m.m10/m.m00 - window.width*0.5 );
 
88
        int dy = cvRound( m.m01/m.m00 - window.height*0.5 );
 
89
 
 
90
        int nx = std::min(std::max(cur_rect.x + dx, 0), size.width - cur_rect.width);
 
91
        int ny = std::min(std::max(cur_rect.y + dy, 0), size.height - cur_rect.height);
 
92
 
 
93
        dx = nx - cur_rect.x;
 
94
        dy = ny - cur_rect.y;
 
95
        cur_rect.x = nx;
 
96
        cur_rect.y = ny;
 
97
 
 
98
        // Check for coverage centers mass & window
 
99
        if( dx*dx + dy*dy < eps )
 
100
            break;
 
101
    }
 
102
 
 
103
    window = cur_rect;
 
104
    return i;
 
105
}
 
106
 
 
107
 
 
108
cv::RotatedRect cv::CamShift( InputArray _probImage, Rect& window,
 
109
                              TermCriteria criteria )
 
110
{
 
111
    const int TOLERANCE = 10;
 
112
    Size size;
 
113
    Mat mat;
 
114
    UMat umat;
 
115
    bool isUMat = _probImage.isUMat();
 
116
 
 
117
    if (isUMat)
 
118
        umat = _probImage.getUMat(), size = umat.size();
 
119
    else
 
120
        mat = _probImage.getMat(), size = mat.size();
 
121
 
 
122
    meanShift( _probImage, window, criteria );
 
123
 
 
124
    window.x -= TOLERANCE;
 
125
    if( window.x < 0 )
 
126
        window.x = 0;
 
127
 
 
128
    window.y -= TOLERANCE;
 
129
    if( window.y < 0 )
 
130
        window.y = 0;
 
131
 
 
132
    window.width += 2 * TOLERANCE;
 
133
    if( window.x + window.width > size.width )
 
134
        window.width = size.width - window.x;
 
135
 
 
136
    window.height += 2 * TOLERANCE;
 
137
    if( window.y + window.height > size.height )
 
138
        window.height = size.height - window.y;
 
139
 
 
140
    // Calculating moments in new center mass
 
141
    Moments m = isUMat ? moments(umat(window)) : moments(mat(window));
 
142
 
 
143
    double m00 = m.m00, m10 = m.m10, m01 = m.m01;
 
144
    double mu11 = m.mu11, mu20 = m.mu20, mu02 = m.mu02;
 
145
 
 
146
    if( fabs(m00) < DBL_EPSILON )
 
147
        return RotatedRect();
 
148
 
 
149
    double inv_m00 = 1. / m00;
 
150
    int xc = cvRound( m10 * inv_m00 + window.x );
 
151
    int yc = cvRound( m01 * inv_m00 + window.y );
 
152
    double a = mu20 * inv_m00, b = mu11 * inv_m00, c = mu02 * inv_m00;
 
153
 
 
154
    // Calculating width & height
 
155
    double square = std::sqrt( 4 * b * b + (a - c) * (a - c) );
 
156
 
 
157
    // Calculating orientation
 
158
    double theta = atan2( 2 * b, a - c + square );
 
159
 
 
160
    // Calculating width & length of figure
 
161
    double cs = cos( theta );
 
162
    double sn = sin( theta );
 
163
 
 
164
    double rotate_a = cs * cs * mu20 + 2 * cs * sn * mu11 + sn * sn * mu02;
 
165
    double rotate_c = sn * sn * mu20 - 2 * cs * sn * mu11 + cs * cs * mu02;
 
166
    double length = std::sqrt( rotate_a * inv_m00 ) * 4;
 
167
    double width = std::sqrt( rotate_c * inv_m00 ) * 4;
 
168
 
 
169
    // In case, when tetta is 0 or 1.57... the Length & Width may be exchanged
 
170
    if( length < width )
 
171
    {
 
172
        std::swap( length, width );
 
173
        std::swap( cs, sn );
 
174
        theta = CV_PI*0.5 - theta;
 
175
    }
 
176
 
 
177
    // Saving results
 
178
    int _xc = cvRound( xc );
 
179
    int _yc = cvRound( yc );
 
180
 
 
181
    int t0 = cvRound( fabs( length * cs ));
 
182
    int t1 = cvRound( fabs( width * sn ));
 
183
 
 
184
    t0 = MAX( t0, t1 ) + 2;
 
185
    window.width = MIN( t0, (size.width - _xc) * 2 );
 
186
 
 
187
    t0 = cvRound( fabs( length * sn ));
 
188
    t1 = cvRound( fabs( width * cs ));
 
189
 
 
190
    t0 = MAX( t0, t1 ) + 2;
 
191
    window.height = MIN( t0, (size.height - _yc) * 2 );
 
192
 
 
193
    window.x = MAX( 0, _xc - window.width / 2 );
 
194
    window.y = MAX( 0, _yc - window.height / 2 );
 
195
 
 
196
    window.width = MIN( size.width - window.x, window.width );
 
197
    window.height = MIN( size.height - window.y, window.height );
 
198
 
 
199
    RotatedRect box;
 
200
    box.size.height = (float)length;
 
201
    box.size.width = (float)width;
 
202
    box.angle = (float)((CV_PI*0.5+theta)*180./CV_PI);
 
203
    while(box.angle < 0)
 
204
        box.angle += 360;
 
205
    while(box.angle >= 360)
 
206
        box.angle -= 360;
 
207
    if(box.angle >= 180)
 
208
        box.angle -= 180;
 
209
    box.center = Point2f( window.x + window.width*0.5f, window.y + window.height*0.5f);
 
210
 
 
211
    return box;
 
212
}
 
213
 
 
214
/* End of file. */