vcs.maemo.org Git - opencv/blob - src/cvaux/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     SetModuleName("Kalman");
 132 }
 133
 134 CvBlobTrackPostProcKalman::~CvBlobTrackPostProcKalman()
 135 {
 136     cvReleaseKalman(&m_pKalman);
 137 }
 138
 139 void CvBlobTrackPostProcKalman::ParamUpdate()
 140 {
 141     cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
 142     cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
 143     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
 144     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
 145 }
 146
 147 CvBlob* CvBlobTrackPostProcKalman::Process(CvBlob* pBlob)
 148 {
 149     CvBlob* pBlobRes = &m_Blob;
 150     float   Z[4];
 151     CvMat   Zmat = cvMat(4,1,CV_32F,Z);
 152     m_Blob = pBlob[0];
 153
 154     if(m_Frame < 2)
 155     {   /* First call: */
 156         m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
 157         m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
 158         if(m_pKalman->DP>6)
 159         {
 160             m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
 161             m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
 162         }
 163         m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
 164         m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
 165         m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
 166         m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
 167     }
 168     else
 169     {   /* Nonfirst call: */
 170         cvKalmanPredict(m_pKalman,0);
 171         Z[0] = CV_BLOB_X(pBlob);
 172         Z[1] = CV_BLOB_Y(pBlob);
 173         Z[2] = CV_BLOB_WX(pBlob);
 174         Z[3] = CV_BLOB_WY(pBlob);
 175         cvKalmanCorrect(m_pKalman,&Zmat);
 176         cvMatMulAdd(m_pKalman->measurement_matrix, m_pKalman->state_post, NULL, &Zmat);
 177         CV_BLOB_X(pBlobRes) = Z[0];
 178         CV_BLOB_Y(pBlobRes) = Z[1];
 179 //        CV_BLOB_WX(pBlobRes) = Z[2];
 180 //        CV_BLOB_WY(pBlobRes) = Z[3];
 181     }
 182     m_Frame++;
 183     return pBlobRes;
 184 }
 185
 186 void CvBlobTrackPostProcKalman::Release()
 187 {
 188     delete this;
 189 }
 190
 191 CvBlobTrackPostProcOne* cvCreateModuleBlobTrackPostProcKalmanOne()
 192 {
 193     return (CvBlobTrackPostProcOne*) new CvBlobTrackPostProcKalman;
 194 }
 195
 196 CvBlobTrackPostProc* cvCreateModuleBlobTrackPostProcKalman()
 197 {
 198     return cvCreateBlobTrackPostProcList(cvCreateModuleBlobTrackPostProcKalmanOne);
 199 }
 200 /*======================= KALMAN FILTER =========================*/
 201
 202
 203
 204 /*======================= KALMAN PREDICTOR =========================*/
 205 class CvBlobTrackPredictKalman:public CvBlobTrackPredictor
 206 {
 207
 208 private:
 209     CvBlob      m_BlobPredict;
 210     CvKalman*   m_pKalman;
 211     int         m_Frame;
 212     float       m_ModelNoise;
 213     float       m_DataNoisePos;
 214     float       m_DataNoiseSize;
 215
 216 public:
 217     CvBlobTrackPredictKalman();
 218     ~CvBlobTrackPredictKalman();
 219     CvBlob* Predict();
 220     void Update(CvBlob* pBlob);
 221     virtual void ParamUpdate();
 222     void Release()
 223     {
 224         delete this;
 225     }
 226 };  /* class CvBlobTrackPredictKalman */
 227
 228
 229 void CvBlobTrackPredictKalman::ParamUpdate()
 230 {
 231     cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
 232     cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
 233     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
 234     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
 235 }
 236
 237 CvBlobTrackPredictKalman::CvBlobTrackPredictKalman()
 238 {
 239     m_ModelNoise = 1e-6f;
 240     m_DataNoisePos = 1e-6f;
 241     m_DataNoiseSize = 1e-1f;
 242
 243     #if STATE_NUM>6
 244         m_DataNoiseSize *= (float)pow(20.,2.);
 245     #else
 246         m_DataNoiseSize /= (float)pow(20.,2.);
 247     #endif
 248
 249     AddParam("ModelNoise",&m_ModelNoise);
 250     AddParam("DataNoisePos",&m_DataNoisePos);
 251     AddParam("DataNoiseSize",&m_DataNoiseSize);
 252
 253     m_Frame = 0;
 254     m_pKalman = cvCreateKalman(STATE_NUM,4);
 255     memcpy( m_pKalman->transition_matrix->data.fl, A, sizeof(A));
 256     memcpy( m_pKalman->measurement_matrix->data.fl, H, sizeof(H));
 257
 258     cvSetIdentity( m_pKalman->process_noise_cov, cvRealScalar(m_ModelNoise) );
 259     cvSetIdentity( m_pKalman->measurement_noise_cov, cvRealScalar(m_DataNoisePos) );
 260     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 2,2) = m_DataNoiseSize;
 261     CV_MAT_ELEM(*m_pKalman->measurement_noise_cov, float, 3,3) = m_DataNoiseSize;
 262     cvSetIdentity( m_pKalman->error_cov_post, cvRealScalar(1));
 263     cvZero(m_pKalman->state_post);
 264     cvZero(m_pKalman->state_pre);
 265
 266     SetModuleName("Kalman");
 267 }
 268
 269 CvBlobTrackPredictKalman::~CvBlobTrackPredictKalman()
 270 {
 271     cvReleaseKalman(&m_pKalman);
 272 }
 273
 274 CvBlob* CvBlobTrackPredictKalman::Predict()
 275 {
 276     if(m_Frame >= 2)
 277     {
 278         cvKalmanPredict(m_pKalman,0);
 279         m_BlobPredict.x = m_pKalman->state_pre->data.fl[0];
 280         m_BlobPredict.y = m_pKalman->state_pre->data.fl[1];
 281         m_BlobPredict.w = m_pKalman->state_pre->data.fl[2];
 282         m_BlobPredict.h = m_pKalman->state_pre->data.fl[3];
 283     }
 284     return &m_BlobPredict;
 285 }
 286
 287 void CvBlobTrackPredictKalman::Update(CvBlob* pBlob)
 288 {
 289     float   Z[4];
 290     CvMat   Zmat = cvMat(4,1,CV_32F,Z);
 291     m_BlobPredict = pBlob[0];
 292
 293     if(m_Frame < 2)
 294     {   /* First call: */
 295         m_pKalman->state_post->data.fl[0+4] = CV_BLOB_X(pBlob)-m_pKalman->state_post->data.fl[0];
 296         m_pKalman->state_post->data.fl[1+4] = CV_BLOB_Y(pBlob)-m_pKalman->state_post->data.fl[1];
 297         if(m_pKalman->DP>6)
 298         {
 299             m_pKalman->state_post->data.fl[2+4] = CV_BLOB_WX(pBlob)-m_pKalman->state_post->data.fl[2];
 300             m_pKalman->state_post->data.fl[3+4] = CV_BLOB_WY(pBlob)-m_pKalman->state_post->data.fl[3];
 301         }
 302         m_pKalman->state_post->data.fl[0] = CV_BLOB_X(pBlob);
 303         m_pKalman->state_post->data.fl[1] = CV_BLOB_Y(pBlob);
 304         m_pKalman->state_post->data.fl[2] = CV_BLOB_WX(pBlob);
 305         m_pKalman->state_post->data.fl[3] = CV_BLOB_WY(pBlob);
 306     }
 307     else
 308     {   /* Nonfirst call: */
 309         Z[0] = CV_BLOB_X(pBlob);
 310         Z[1] = CV_BLOB_Y(pBlob);
 311         Z[2] = CV_BLOB_WX(pBlob);
 312         Z[3] = CV_BLOB_WY(pBlob);
 313         cvKalmanCorrect(m_pKalman,&Zmat);
 314     }
 315
 316     cvKalmanPredict(m_pKalman,0);
 317
 318     m_Frame++;
 319
 320 }   /* Update. */
 321
 322 CvBlobTrackPredictor* cvCreateModuleBlobTrackPredictKalman()
 323 {
 324     return (CvBlobTrackPredictor*) new CvBlobTrackPredictKalman;
 325 }
 326 /*======================= KALMAN PREDICTOR =========================*/
 327