git.maemo.org Git - opencv/blob - cvaux/src/vs/blobtrackpostprockalman.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 //                        Intel License Agreement
  11 //
  12 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  13 // Third party copyrights are property of their respective owners.
  14 //
  15 // Redistribution and use in source and binary forms, with or without modification,
  16 // are permitted provided that the following conditions are met:
  17 //
  18 //   * Redistribution's of source code must retain the above copyright notice,
  19 //     this list of conditions and the following disclaimer.
  20 //
  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.
  24 //
  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.
  27 //
  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.
  38 //
  39 //M*/
  40
  41 #include "_cvaux.h"
  42
  43 /*======================= KALMAN FILTER =========================*/
  44 /* State vector is (x,y,w,h,dx,dy,dw,dh). */
  45 /* Measurement is (x,y,w,h). */
  46
  47 /* Dynamic matrix A: */
  48 const float A8[] = { 1, 0, 0, 0, 1, 0, 0, 0,
  49                      0, 1, 0, 0, 0, 1, 0, 0,
  50                      0, 0, 1, 0, 0, 0, 1, 0,
  51                      0, 0, 0, 1, 0, 0, 0, 1,
  52                      0, 0, 0, 0, 1, 0, 0, 0,
  53                      0, 0, 0, 0, 0, 1, 0, 0,
  54                      0, 0, 0, 0, 0, 0, 1, 0,
  55                      0, 0, 0, 0, 0, 0, 0, 1};
  56
  57 /* Measurement matrix H: */
  58 const float H8[] = { 1, 0, 0, 0, 0, 0, 0, 0,
  59                      0, 1, 0, 0, 0, 0, 0, 0,
  60                      0, 0, 1, 0, 0, 0, 0, 0,
  61                      0, 0, 0, 1, 0, 0, 0, 0};
  62
  63 /* Matrices for zero size velocity: */
  64 /* Dinamic matrix A: */
  65 const float A6[] = { 1, 0, 0, 0, 1, 0,
  66                      0, 1, 0, 0, 0, 1,
  67                      0, 0, 1, 0, 0, 0,
  68                      0, 0, 0, 1, 0, 0,
  69                      0, 0, 0, 0, 1, 0,
  70                      0, 0, 0, 0, 0, 1};
  71
  72 /* Measurement matrix H: */
  73 const float H6[] = { 1, 0, 0, 0, 0, 0,
  74                      0, 1, 0, 0, 0, 0,
  75                      0, 0, 1, 0, 0, 0,
  76                      0, 0, 0, 1, 0, 0};
  77
  78 #define STATE_NUM 6
  79 #define A A6
  80 #define H H6
  81
  82 class CvBlobTrackPostProcKalman:public CvBlobTrackPostProcOne
  83 {
  84
  85 private:
  86     CvBlob      m_Blob;
  87     CvKalman*   m_pKalman;
  88     int         m_Frame;
  89     float       m_ModelNoise;
  90     float       m_DataNoisePos;
  91     float       m_DataNoiseSize;
  92
  93 public:
  94     CvBlobTrackPostProcKalman();
  95    ~CvBlobTrackPostProcKalman();
  96     CvBlob* Process(CvBlob* pBlob);
  97     void Release();
  98     virtual void ParamUpdate();
  99 }; /* class CvBlobTrackPostProcKalman */
 100
 101
 102 CvBlobTrackPostProcKalman::CvBlobTrackPostProcKalman()
 103 {
 104     m_ModelNoise = 1e-6f;
 105     m_DataNoisePos = 1e-6f;
 106     m_DataNoiseSize = 1e-1f;
 107
 108     #if STATE_NUM>6
 109         m_DataNoiseSize *= (float)pow(20.,2.);
 110     #else
 111         m_DataNoiseSize /= (float)pow(20.,2.);
 112     #endif
 113
 114     AddParam("ModelNoise",&m_ModelNoise);
 115     AddParam("DataNoisePos",&m_DataNoisePos);
 116     AddParam("DataNoiseSize",&m_DataNoiseSize);
 117
 118     m_Frame = 0;
 119     m_pKalman = cvCreateKalman(STATE_NUM,4);
 120     memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A));
 121     memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H));
 122
 123     cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
 124     cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
 125     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
 126     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
 127     cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1));
 128     cvZero(m_pKalman->state_post);
 129     cvZero(m_pKalman->state_pre);
 130 }
 131
 132 CvBlobTrackPostProcKalman::~CvBlobTrackPostProcKalman()
 133 {
 134     cvReleaseKalman(&m_pKalman);
 135 }
 136
 137 void CvBlobTrackPostProcKalman::ParamUpdate()
 138 {
 139     cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
 140     cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
 141     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
 142     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
 143 }
 144
 145 CvBlob* CvBlobTrackPostProcKalman::Process(CvBlob* pBlob)
 146 {
 147     CvBlob* pBlobRes = &m_Blob;
 148     float   Z[4];
 149     CvMat   Zmat = cvMat(4,1,CV_32F,Z);
 150     m_Blob = pBlob[0];
 151
 152     if(m_Frame < 2)
 153     {   /* First call: */
 154         m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
 155         m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
 156         if(m_pKalman->DP>6)
 157         {
 158             m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
 159             m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
 160         }
 161         m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
 162         m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
 163         m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
 164         m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
 165     }
 166     else
 167     {   /* Nonfirst call: */
 168         cvKalmanPredict(m_pKalman,0);
 169         Z[0] = CV_BLOB_X(pBlob);
 170         Z[1] = CV_BLOB_Y(pBlob);
 171         Z[2] = CV_BLOB_WX(pBlob);
 172         Z[3] = CV_BLOB_WY(pBlob);
 173         cvKalmanCorrect(m_pKalman,&Zmat);
 174         cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_post, NULL, &Zmat);
 175         CV_BLOB_X(pBlobRes) = Z[0];
 176         CV_BLOB_Y(pBlobRes) = Z[1];
 177 //        CV_BLOB_WX(pBlobRes) = Z[2];
 178 //        CV_BLOB_WY(pBlobRes) = Z[3];
 179     }
 180     m_Frame++;
 181     return pBlobRes;
 182 }
 183
 184 void CvBlobTrackPostProcKalman::Release()
 185 {
 186     delete this;
 187 }
 188
 189 CvBlobTrackPostProcOne* cvCreateModuleBlobTrackPostProcKalmanOne()
 190 {
 191     return (CvBlobTrackPostProcOne*) new CvBlobTrackPostProcKalman;
 192 }
 193
 194 CvBlobTrackPostProc* cvCreateModuleBlobTrackPostProcKalman()
 195 {
 196     return cvCreateBlobTrackPostProcList(cvCreateModuleBlobTrackPostProcKalmanOne);
 197 }
 198 /*======================= KALMAN FILTER =========================*/
 199
 200
 201
 202 /*======================= KALMAN PREDICTOR =========================*/
 203 class CvBlobTrackPredictKalman:public CvBlobTrackPredictor
 204 {
 205
 206 private:
 207     CvBlob      m_BlobPredict;
 208     CvKalman*   m_pKalman;
 209     int         m_Frame;
 210     float       m_ModelNoise;
 211     float       m_DataNoisePos;
 212     float       m_DataNoiseSize;
 213
 214 public:
 215     CvBlobTrackPredictKalman();
 216     ~CvBlobTrackPredictKalman();
 217     CvBlob* Predict();
 218     void Update(CvBlob* pBlob);
 219     virtual void ParamUpdate();
 220     void Release()
 221     {
 222         delete this;
 223     }
 224 };  /* class CvBlobTrackPredictKalman */
 225
 226
 227 void CvBlobTrackPredictKalman::ParamUpdate()
 228 {
 229     cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
 230     cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
 231     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
 232     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
 233 }
 234
 235 CvBlobTrackPredictKalman::CvBlobTrackPredictKalman()
 236 {
 237     m_ModelNoise = 1e-6f;
 238     m_DataNoisePos = 1e-6f;
 239     m_DataNoiseSize = 1e-1f;
 240
 241     #if STATE_NUM>6
 242         m_DataNoiseSize *= (float)pow(20.,2.);
 243     #else
 244         m_DataNoiseSize /= (float)pow(20.,2.);
 245     #endif
 246
 247     AddParam("ModelNoise",&m_ModelNoise);
 248     AddParam("DataNoisePos",&m_DataNoisePos);
 249     AddParam("DataNoiseSize",&m_DataNoiseSize);
 250
 251     m_Frame = 0;
 252     m_pKalman = cvCreateKalman(STATE_NUM,4);
 253     memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A));
 254     memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H));
 255
 256     cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
 257     cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
 258     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
 259     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
 260     cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1));
 261     cvZero(m_pKalman->state_post);
 262     cvZero(m_pKalman->state_pre);
 263 }
 264
 265 CvBlobTrackPredictKalman::~CvBlobTrackPredictKalman()
 266 {
 267     cvReleaseKalman(&m_pKalman);
 268 }
 269
 270 CvBlob* CvBlobTrackPredictKalman::Predict()
 271 {
 272     if(m_Frame >= 2)
 273     {
 274         cvKalmanPredict(m_pKalman,0);
 275         m_BlobPredict.x = m_pKalman->state_pre->data.fl[0];
 276         m_BlobPredict.y = m_pKalman->state_pre->data.fl[1];
 277         m_BlobPredict.w = m_pKalman->state_pre->data.fl[2];
 278         m_BlobPredict.h = m_pKalman->state_pre->data.fl[3];
 279     }
 280     return &m_BlobPredict;
 281 }
 282
 283 void CvBlobTrackPredictKalman::Update(CvBlob* pBlob)
 284 {
 285     float   Z[4];
 286     CvMat   Zmat = cvMat(4,1,CV_32F,Z);
 287     m_BlobPredict = pBlob[0];
 288
 289     if(m_Frame < 2)
 290     {   /* First call: */
 291         m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
 292         m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
 293         if(m_pKalman->DP>6)
 294         {
 295             m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
 296             m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
 297         }
 298         m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
 299         m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
 300         m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
 301         m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
 302     }
 303     else
 304     {   /* Nonfirst call: */
 305         Z[0] = CV_BLOB_X(pBlob);
 306         Z[1] = CV_BLOB_Y(pBlob);
 307         Z[2] = CV_BLOB_WX(pBlob);
 308         Z[3] = CV_BLOB_WY(pBlob);
 309         cvKalmanCorrect(m_pKalman,&Zmat);
 310     }
 311
 312     cvKalmanPredict(m_pKalman,0);
 313
 314     m_Frame++;
 315
 316 }   /* Update. */
 317
 318 CvBlobTrackPredictor* cvCreateModuleBlobTrackPredictKalman()
 319 {
 320     return (CvBlobTrackPredictor*) new CvBlobTrackPredictKalman;
 321 }
 322 /*======================= KALMAN PREDICTOR =========================*/
 323