1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
12 // Copyright (C) 2000, Intel Corporation, all rights reserved.
13 // Third party copyrights are property of their respective owners.
15 // Redistribution and use in source and binary forms, with or without modification,
16 // are permitted provided that the following conditions are met:
18 // * Redistribution's of source code must retain the above copyright notice,
19 // this list of conditions and the following disclaimer.
21 // * Redistribution's in binary form must reproduce the above copyright notice,
22 // this list of conditions and the following disclaimer in the documentation
23 // and/or other materials provided with the distribution.
25 // * The name of Intel Corporation may not be used to endorse or promote products
26 // derived from this software without specific prior written permission.
28 // This software is provided by the copyright holders and contributors "as is" and
29 // any express or implied warranties, including, but not limited to, the implied
30 // warranties of merchantability and fitness for a particular purpose are disclaimed.
31 // In no event shall the Intel Corporation or contributors be liable for any direct,
32 // indirect, incidental, special, exemplary, or consequential damages
33 // (including, but not limited to, procurement of substitute goods or services;
34 // loss of use, data, or profits; or business interruption) however caused
35 // and on any theory of liability, whether in contract, strict liability,
36 // or tort (including negligence or otherwise) arising in any way out of
37 // the use of this software, even if advised of the possibility of such damage.
43 /*======================= KALMAN FILTER =========================*/
44 /* state vector is (x,y,w,h,dx,dy,dw,dh)*/
45 /* mesurment is (x,y,w,h) */
46 /* dinamic matrix A */
47 const float A8[] = { 1, 0, 0, 0, 1, 0, 0, 0,
48 0, 1, 0, 0, 0, 1, 0, 0,
49 0, 0, 1, 0, 0, 0, 1, 0,
50 0, 0, 0, 1, 0, 0, 0, 1,
51 0, 0, 0, 0, 1, 0, 0, 0,
52 0, 0, 0, 0, 0, 1, 0, 0,
53 0, 0, 0, 0, 0, 0, 1, 0,
54 0, 0, 0, 0, 0, 0, 0, 1};
55 /* measurement matrix H */
56 const float H8[] = { 1, 0, 0, 0, 0, 0, 0, 0,
57 0, 1, 0, 0, 0, 0, 0, 0,
58 0, 0, 1, 0, 0, 0, 0, 0,
59 0, 0, 0, 1, 0, 0, 0, 0};
61 /* matices for zero size velosity */
62 /* dinamic matrix A */
63 const float A6[] = { 1, 0, 0, 0, 1, 0,
69 /* measurement matrix H */
70 const float H6[] = { 1, 0, 0, 0, 0, 0,
79 class CvBlobTrackPostProcKalman:public CvBlobTrackPostProcOne
87 float m_DataNoiseSize;
89 CvBlobTrackPostProcKalman();
90 ~CvBlobTrackPostProcKalman();
91 CvBlob* Process(CvBlob* pBlob);
93 virtual void ParamUpdate();
94 }; /* class CvBlobTrackPostProcKalman */
97 CvBlobTrackPostProcKalman::CvBlobTrackPostProcKalman()
100 m_DataNoisePos = 1e-6f;
101 m_DataNoiseSize = 1e-1f;
103 m_DataNoiseSize *= (float)pow(20.,2.);
105 m_DataNoiseSize /= (float)pow(20.,2.);
108 AddParam("ModelNoise",&m_ModelNoise);
109 AddParam("DataNoisePos",&m_DataNoisePos);
110 AddParam("DataNoiseSize",&m_DataNoiseSize);
113 m_pKalman = cvCreateKalman(STATE_NUM,4);
114 memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A));
115 memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H));
117 cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
118 cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
119 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
120 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
121 cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1));
122 cvZero(m_pKalman->state_post);
123 cvZero(m_pKalman->state_pre);
125 CvBlobTrackPostProcKalman::~CvBlobTrackPostProcKalman()
127 cvReleaseKalman(&m_pKalman);
130 void CvBlobTrackPostProcKalman::ParamUpdate()
132 cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
133 cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
134 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
135 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
137 CvBlob* CvBlobTrackPostProcKalman::Process(CvBlob* pBlob)
139 CvBlob* pBlobRes = &m_Blob;
141 CvMat Zmat = cvMat(4,1,CV_32F,Z);
145 m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
146 m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
149 m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
150 m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
152 m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
153 m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
154 m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
155 m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
159 cvKalmanPredict(m_pKalman,0);
160 Z[0] = CV_BLOB_X(pBlob);
161 Z[1] = CV_BLOB_Y(pBlob);
162 Z[2] = CV_BLOB_WX(pBlob);
163 Z[3] = CV_BLOB_WY(pBlob);
164 cvKalmanCorrect(m_pKalman,&Zmat);
165 cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_post, NULL, &Zmat);
166 CV_BLOB_X(pBlobRes) = Z[0];
167 CV_BLOB_Y(pBlobRes) = Z[1];
168 // CV_BLOB_WX(pBlobRes) = Z[2];
169 // CV_BLOB_WY(pBlobRes) = Z[3];
174 void CvBlobTrackPostProcKalman::Release()
179 CvBlobTrackPostProcOne* cvCreateModuleBlobTrackPostProcKalmanOne()
181 return (CvBlobTrackPostProcOne*) new CvBlobTrackPostProcKalman;
184 CvBlobTrackPostProc* cvCreateModuleBlobTrackPostProcKalman()
186 return cvCreateBlobTrackPostProcList(cvCreateModuleBlobTrackPostProcKalmanOne);
188 /*======================= KALMAN FILTER =========================*/
192 /*======================= KALMAN PREDICTOR =========================*/
193 class CvBlobTrackPredictKalman:public CvBlobTrackPredictor
196 CvBlob m_BlobPredict;
200 float m_DataNoisePos;
201 float m_DataNoiseSize;
203 CvBlobTrackPredictKalman();
204 ~CvBlobTrackPredictKalman();
206 void Update(CvBlob* pBlob);
207 virtual void ParamUpdate();
212 }; /* class CvBlobTrackPredictKalman */
215 void CvBlobTrackPredictKalman::ParamUpdate()
217 cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
218 cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
219 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
220 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
222 CvBlobTrackPredictKalman::CvBlobTrackPredictKalman()
224 m_ModelNoise = 1e-6f;
225 m_DataNoisePos = 1e-6f;
226 m_DataNoiseSize = 1e-1f;
228 m_DataNoiseSize *= (float)pow(20.,2.);
230 m_DataNoiseSize /= (float)pow(20.,2.);
233 AddParam("ModelNoise",&m_ModelNoise);
234 AddParam("DataNoisePos",&m_DataNoisePos);
235 AddParam("DataNoiseSize",&m_DataNoiseSize);
238 m_pKalman = cvCreateKalman(STATE_NUM,4);
239 memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A));
240 memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H));
242 cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
243 cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
244 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
245 CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
246 cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1));
247 cvZero(m_pKalman->state_post);
248 cvZero(m_pKalman->state_pre);
250 CvBlobTrackPredictKalman::~CvBlobTrackPredictKalman()
252 cvReleaseKalman(&m_pKalman);
254 CvBlob* CvBlobTrackPredictKalman::Predict()
258 cvKalmanPredict(m_pKalman,0);
259 m_BlobPredict.x = m_pKalman->state_pre->data.fl[0];
260 m_BlobPredict.y = m_pKalman->state_pre->data.fl[1];
261 m_BlobPredict.w = m_pKalman->state_pre->data.fl[2];
262 m_BlobPredict.h = m_pKalman->state_pre->data.fl[3];
264 return &m_BlobPredict;
267 void CvBlobTrackPredictKalman::Update(CvBlob* pBlob)
270 CvMat Zmat = cvMat(4,1,CV_32F,Z);
271 m_BlobPredict = pBlob[0];
274 m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
275 m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
278 m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
279 m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
281 m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
282 m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
283 m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
284 m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
288 Z[0] = CV_BLOB_X(pBlob);
289 Z[1] = CV_BLOB_Y(pBlob);
290 Z[2] = CV_BLOB_WX(pBlob);
291 Z[3] = CV_BLOB_WY(pBlob);
292 cvKalmanCorrect(m_pKalman,&Zmat);
294 cvKalmanPredict(m_pKalman,0);
298 CvBlobTrackPredictor* cvCreateModuleBlobTrackPredictKalman()
300 return (CvBlobTrackPredictor*) new CvBlobTrackPredictKalman;
302 /*======================= KALMAN PREDICTOR =========================*/