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