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