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 the copyright holders 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
43 #include "precomp.hpp"
44 #include "opencv2/video/tracking_c.h"
45
46
47 /////////////////////////// Meanshift & CAMShift ///////////////////////////
48
49 CV_IMPL int
cvMeanShift(const void * imgProb,CvRect windowIn,CvTermCriteria criteria,CvConnectedComp * comp)50 cvMeanShift( const void* imgProb, CvRect windowIn,
51 CvTermCriteria criteria, CvConnectedComp* comp )
52 {
53 cv::Mat img = cv::cvarrToMat(imgProb);
54 cv::Rect window = windowIn;
55 int iters = cv::meanShift(img, window, criteria);
56
57 if( comp )
58 {
59 comp->rect = window;
60 comp->area = cvRound(cv::sum(img(window))[0]);
61 }
62
63 return iters;
64 }
65
66
67 CV_IMPL int
cvCamShift(const void * imgProb,CvRect windowIn,CvTermCriteria criteria,CvConnectedComp * comp,CvBox2D * box)68 cvCamShift( const void* imgProb, CvRect windowIn,
69 CvTermCriteria criteria,
70 CvConnectedComp* comp,
71 CvBox2D* box )
72 {
73 cv::Mat img = cv::cvarrToMat(imgProb);
74 cv::Rect window = windowIn;
75 cv::RotatedRect rr = cv::CamShift(img, window, criteria);
76
77 if( comp )
78 {
79 comp->rect = window;
80 cv::Rect roi = rr.boundingRect() & cv::Rect(0, 0, img.cols, img.rows);
81 comp->area = cvRound(cv::sum(img(roi))[0]);
82 }
83
84 if( box )
85 *box = rr;
86
87 return rr.size.width*rr.size.height > 0.f ? 1 : -1;
88 }
89
90 ///////////////////////////////// Kalman ///////////////////////////////
91
92 CV_IMPL CvKalman*
cvCreateKalman(int DP,int MP,int CP)93 cvCreateKalman( int DP, int MP, int CP )
94 {
95 CvKalman *kalman = 0;
96
97 if( DP <= 0 || MP <= 0 )
98 CV_Error( CV_StsOutOfRange,
99 "state and measurement vectors must have positive number of dimensions" );
100
101 if( CP < 0 )
102 CP = DP;
103
104 /* allocating memory for the structure */
105 kalman = (CvKalman *)cvAlloc( sizeof( CvKalman ));
106 memset( kalman, 0, sizeof(*kalman));
107
108 kalman->DP = DP;
109 kalman->MP = MP;
110 kalman->CP = CP;
111
112 kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 );
113 cvZero( kalman->state_pre );
114
115 kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 );
116 cvZero( kalman->state_post );
117
118 kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 );
119 cvSetIdentity( kalman->transition_matrix );
120
121 kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 );
122 cvSetIdentity( kalman->process_noise_cov );
123
124 kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 );
125 cvZero( kalman->measurement_matrix );
126
127 kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 );
128 cvSetIdentity( kalman->measurement_noise_cov );
129
130 kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 );
131
132 kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 );
133 cvZero( kalman->error_cov_post );
134
135 kalman->gain = cvCreateMat( DP, MP, CV_32FC1 );
136
137 if( CP > 0 )
138 {
139 kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 );
140 cvZero( kalman->control_matrix );
141 }
142
143 kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 );
144 kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 );
145 kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 );
146 kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 );
147 kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 );
148
149 #if 1
150 kalman->PosterState = kalman->state_pre->data.fl;
151 kalman->PriorState = kalman->state_post->data.fl;
152 kalman->DynamMatr = kalman->transition_matrix->data.fl;
153 kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;
154 kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;
155 kalman->PNCovariance = kalman->process_noise_cov->data.fl;
156 kalman->KalmGainMatr = kalman->gain->data.fl;
157 kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;
158 kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;
159 #endif
160
161 return kalman;
162 }
163
164
165 CV_IMPL void
cvReleaseKalman(CvKalman ** _kalman)166 cvReleaseKalman( CvKalman** _kalman )
167 {
168 CvKalman *kalman;
169
170 if( !_kalman )
171 CV_Error( CV_StsNullPtr, "" );
172
173 kalman = *_kalman;
174 if( !kalman )
175 return;
176
177 /* freeing the memory */
178 cvReleaseMat( &kalman->state_pre );
179 cvReleaseMat( &kalman->state_post );
180 cvReleaseMat( &kalman->transition_matrix );
181 cvReleaseMat( &kalman->control_matrix );
182 cvReleaseMat( &kalman->measurement_matrix );
183 cvReleaseMat( &kalman->process_noise_cov );
184 cvReleaseMat( &kalman->measurement_noise_cov );
185 cvReleaseMat( &kalman->error_cov_pre );
186 cvReleaseMat( &kalman->gain );
187 cvReleaseMat( &kalman->error_cov_post );
188 cvReleaseMat( &kalman->temp1 );
189 cvReleaseMat( &kalman->temp2 );
190 cvReleaseMat( &kalman->temp3 );
191 cvReleaseMat( &kalman->temp4 );
192 cvReleaseMat( &kalman->temp5 );
193
194 memset( kalman, 0, sizeof(*kalman));
195
196 /* deallocating the structure */
197 cvFree( _kalman );
198 }
199
200
201 CV_IMPL const CvMat*
cvKalmanPredict(CvKalman * kalman,const CvMat * control)202 cvKalmanPredict( CvKalman* kalman, const CvMat* control )
203 {
204 if( !kalman )
205 CV_Error( CV_StsNullPtr, "" );
206
207 /* update the state */
208 /* x'(k) = A*x(k) */
209 cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre );
210
211 if( control && kalman->CP > 0 )
212 /* x'(k) = x'(k) + B*u(k) */
213 cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre );
214
215 /* update error covariance matrices */
216 /* temp1 = A*P(k) */
217 cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 );
218
219 /* P'(k) = temp1*At + Q */
220 cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,
221 kalman->error_cov_pre, CV_GEMM_B_T );
222
223 /* handle the case when there will be measurement before the next predict */
224 cvCopy(kalman->state_pre, kalman->state_post);
225
226 return kalman->state_pre;
227 }
228
229
230 CV_IMPL const CvMat*
cvKalmanCorrect(CvKalman * kalman,const CvMat * measurement)231 cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )
232 {
233 if( !kalman || !measurement )
234 CV_Error( CV_StsNullPtr, "" );
235
236 /* temp2 = H*P'(k) */
237 cvMatMulAdd( kalman->measurement_matrix, kalman->error_cov_pre, 0, kalman->temp2 );
238 /* temp3 = temp2*Ht + R */
239 cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,
240 kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T );
241
242 /* temp4 = inv(temp3)*temp2 = Kt(k) */
243 cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD );
244
245 /* K(k) */
246 cvTranspose( kalman->temp4, kalman->gain );
247
248 /* temp5 = z(k) - H*x'(k) */
249 cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 );
250
251 /* x(k) = x'(k) + K(k)*temp5 */
252 cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post );
253
254 /* P(k) = P'(k) - K(k)*temp2 */
255 cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,
256 kalman->error_cov_post, 0 );
257
258 return kalman->state_post;
259 }
260
261 ///////////////////////////////////// Optical Flow ////////////////////////////////
262
263 CV_IMPL void
cvCalcOpticalFlowPyrLK(const void * arrA,const void * arrB,void *,void *,const CvPoint2D32f * featuresA,CvPoint2D32f * featuresB,int count,CvSize winSize,int level,char * status,float * error,CvTermCriteria criteria,int flags)264 cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB,
265 void* /*pyrarrA*/, void* /*pyrarrB*/,
266 const CvPoint2D32f * featuresA,
267 CvPoint2D32f * featuresB,
268 int count, CvSize winSize, int level,
269 char *status, float *error,
270 CvTermCriteria criteria, int flags )
271 {
272 if( count <= 0 )
273 return;
274 CV_Assert( featuresA && featuresB );
275 cv::Mat A = cv::cvarrToMat(arrA), B = cv::cvarrToMat(arrB);
276 cv::Mat ptA(count, 1, CV_32FC2, (void*)featuresA);
277 cv::Mat ptB(count, 1, CV_32FC2, (void*)featuresB);
278 cv::Mat st, err;
279
280 if( status )
281 st = cv::Mat(count, 1, CV_8U, (void*)status);
282 if( error )
283 err = cv::Mat(count, 1, CV_32F, (void*)error);
284 cv::calcOpticalFlowPyrLK( A, B, ptA, ptB, st,
285 error ? cv::_OutputArray(err) : (cv::_OutputArray)cv::noArray(),
286 winSize, level, criteria, flags);
287 }
288
289
cvCalcOpticalFlowFarneback(const CvArr * _prev,const CvArr * _next,CvArr * _flow,double pyr_scale,int levels,int winsize,int iterations,int poly_n,double poly_sigma,int flags)290 CV_IMPL void cvCalcOpticalFlowFarneback(
291 const CvArr* _prev, const CvArr* _next,
292 CvArr* _flow, double pyr_scale, int levels,
293 int winsize, int iterations, int poly_n,
294 double poly_sigma, int flags )
295 {
296 cv::Mat prev = cv::cvarrToMat(_prev), next = cv::cvarrToMat(_next);
297 cv::Mat flow = cv::cvarrToMat(_flow);
298 CV_Assert( flow.size() == prev.size() && flow.type() == CV_32FC2 );
299 cv::calcOpticalFlowFarneback( prev, next, flow, pyr_scale, levels,
300 winsize, iterations, poly_n, poly_sigma, flags );
301 }
302
303
304 CV_IMPL int
cvEstimateRigidTransform(const CvArr * arrA,const CvArr * arrB,CvMat * arrM,int full_affine)305 cvEstimateRigidTransform( const CvArr* arrA, const CvArr* arrB, CvMat* arrM, int full_affine )
306 {
307 cv::Mat matA = cv::cvarrToMat(arrA), matB = cv::cvarrToMat(arrB);
308 const cv::Mat matM0 = cv::cvarrToMat(arrM);
309
310 cv::Mat matM = cv::estimateRigidTransform(matA, matB, full_affine != 0);
311 if( matM.empty() )
312 {
313 matM = cv::cvarrToMat(arrM);
314 matM.setTo(cv::Scalar::all(0));
315 return 0;
316 }
317 matM.convertTo(matM0, matM0.type());
318 return 1;
319 }
320