git.maemo.org Git - opencv/blob - cvaux/src/vs/blobtrackanalysishist.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 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cvaux.h"
  43
  44 #define MAX_FV_SIZE 5
  45 #define BLOB_NUM    5
  46 typedef struct DefBlobFVN
  47 {
  48     CvBlob  blob;
  49     CvBlob  BlobSeq[BLOB_NUM];
  50     int     state;
  51     int     LastFrame;
  52     int     FrameNum;
  53 }DefBlobFVN;
  54 class CvBlobTrackFVGenN: public CvBlobTrackFVGen
  55 {
  56 private:
  57     CvBlobSeq       m_BlobList;
  58     CvMemStorage*   m_pMem;
  59     CvSeq*          m_pFVSeq;
  60     float           m_FVMax[MAX_FV_SIZE];
  61     float           m_FVMin[MAX_FV_SIZE];
  62     float           m_FVVar[MAX_FV_SIZE];
  63     int             m_Dim;
  64     CvBlob          m_BlobSeq[BLOB_NUM];
  65     int             m_Frame;
  66     int             m_State;
  67     int             m_LastFrame;
  68     int             m_ClearFlag;
  69     void Clear()
  70     {
  71         if(m_pMem)
  72         {
  73             cvClearMemStorage(m_pMem);
  74             m_pFVSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(float)*(m_Dim+1), m_pMem);
  75             m_ClearFlag = 1;
  76         }
  77     }
  78 public:
  79     CvBlobTrackFVGenN(int dim = 2 ):m_BlobList(sizeof(DefBlobFVN))
  80     {
  81         int i;
  82         assert(dim <= MAX_FV_SIZE);
  83         m_Dim = dim;
  84         for(i=0;i<m_Dim;++i)
  85         {
  86             m_FVVar[i] = 0.01f;
  87             m_FVMax[i] = 1;
  88             m_FVMin[i] = 0;
  89         }
  90         m_Frame = 0;
  91         m_State = 0;
  92         m_pMem = cvCreateMemStorage();
  93         m_pFVSeq = NULL;
  94         Clear();
  95     };
  96     ~CvBlobTrackFVGenN()
  97     {
  98         if(m_pMem)cvReleaseMemStorage(&m_pMem);
  99     };
 100
 101     void AddBlob(CvBlob* pBlob)
 102     {
 103         float       FV[MAX_FV_SIZE+1];
 104         int         i;
 105         DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
 106
 107         if(!m_ClearFlag) Clear();
 108
 109         if(pFVBlob==NULL)
 110         {
 111             DefBlobFVN BlobNew;
 112             BlobNew.blob = pBlob[0];
 113             BlobNew.LastFrame = m_Frame;
 114             BlobNew.state = 0;;
 115             BlobNew.FrameNum = 0;
 116             m_BlobList.AddBlob((CvBlob*)&BlobNew);
 117             pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
 118         }/* add new record if need */
 119         pFVBlob->blob = pBlob[0];
 120
 121         /* shift */
 122         for(i=(BLOB_NUM-1);i>0;--i)
 123         {
 124             pFVBlob->BlobSeq[i] = pFVBlob->BlobSeq[i-1];
 125         }
 126
 127         pFVBlob->BlobSeq[0] = pBlob[0];
 128
 129         if(m_Dim>0)
 130         {/* calc FV position */
 131             FV[0] = CV_BLOB_X(pBlob);
 132             FV[1] = CV_BLOB_Y(pBlob);
 133         }
 134
 135         if(m_Dim<=2)
 136         {   /* add new FV if position is enought */
 137             *(int*)(FV+m_Dim) = CV_BLOB_ID(pBlob);
 138             cvSeqPush( m_pFVSeq, FV );
 139         }
 140         else if(pFVBlob->FrameNum > BLOB_NUM)
 141         { /* calc velocity for more complex FV */
 142             float       AverVx = 0;
 143             float       AverVy = 0;
 144             { /* average velocity */
 145                 CvBlob* pBlobSeq = pFVBlob->BlobSeq;
 146                 int     i;
 147                 for(i=1;i<BLOB_NUM;++i)
 148                 {
 149                     AverVx += CV_BLOB_X(pBlobSeq+i-1)-CV_BLOB_X(pBlobSeq+i);
 150                     AverVy += CV_BLOB_Y(pBlobSeq+i-1)-CV_BLOB_Y(pBlobSeq+i);
 151                 }
 152                 AverVx /= BLOB_NUM-1;
 153                 AverVy /= BLOB_NUM-1;
 154
 155                 FV[2] = AverVx;
 156                 FV[3] = AverVy;
 157             }
 158
 159             if(m_Dim>4)
 160             { /* state duration */
 161                 float T = (CV_BLOB_WX(pBlob)+CV_BLOB_WY(pBlob))*0.01f;
 162
 163                 if( fabs(AverVx) < T && fabs(AverVy) < T)
 164                     pFVBlob->state++;
 165                 else
 166                     pFVBlob->state=0;
 167                 FV[4] = (float)pFVBlob->state;
 168             }/* state duration */
 169             /* add new FV */
 170             *(int*)(FV+m_Dim) = CV_BLOB_ID(pBlob);
 171             cvSeqPush( m_pFVSeq, FV );
 172         }/* if velocity is calculated */
 173
 174         pFVBlob->FrameNum++;
 175         pFVBlob->LastFrame = m_Frame;
 176     };  /* AddBlob */
 177     void Process(IplImage* pImg, IplImage* /*pFG*/)
 178     {
 179         int i;
 180         if(!m_ClearFlag) Clear();
 181         for(i=m_BlobList.GetBlobNum();i>0;--i)
 182         { /* delete unused blob */
 183             DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlob(i-1);
 184             if(pFVBlob->LastFrame < m_Frame)
 185             {
 186                 m_BlobList.DelBlob(i-1);
 187             }
 188         }/* check next blob in list */
 189
 190         m_FVMin[0] = 0;
 191         m_FVMin[1] = 0;
 192         m_FVMax[0] = (float)(pImg->width-1);
 193         m_FVMax[1] = (float)(pImg->height-1);
 194         m_FVVar[0] = m_FVMax[0]*0.01f;
 195         m_FVVar[1] = m_FVMax[1]*0.01f;
 196         m_FVVar[2] = (float)(pImg->width-1)/1440.0f;
 197         m_FVMax[2] = (float)(pImg->width-1)*0.02f;
 198         m_FVMin[2] = -m_FVMax[2];
 199         m_FVVar[3] = (float)(pImg->width-1)/1440.0f;
 200         m_FVMax[3] = (float)(pImg->height-1)*0.02f;
 201         m_FVMin[3] = -m_FVMax[3];
 202         m_FVMax[4] = 25*32.0f; /* max state is 32 sec */
 203         m_FVMin[4] = 0;
 204         m_FVVar[4] = 10;
 205
 206         m_Frame++;
 207         m_ClearFlag = 0;
 208     };
 209     virtual void    Release(){delete this;};
 210     virtual int     GetFVSize(){return m_Dim;};
 211     virtual int     GetFVNum()
 212     {
 213         return m_pFVSeq->total;
 214     };
 215     virtual float*  GetFV(int index, int* pFVID)
 216     {
 217         float* pFV = (float*)cvGetSeqElem( m_pFVSeq, index );
 218         if(pFVID)pFVID[0] = *(int*)(pFV+m_Dim);
 219         return pFV;
 220     };
 221     virtual float*  GetFVMin(){return m_FVMin;}; /* returned pointer to array of minimal values of FV, if return 0 then FVrange is not exist */
 222     virtual float*  GetFVMax(){return m_FVMax;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
 223     virtual float*  GetFVVar(){return m_FVVar;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
 224 };/* CvBlobTrackFVGenN */
 225
 226 CvBlobTrackFVGen* cvCreateFVGenP(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(2);}
 227 CvBlobTrackFVGen* cvCreateFVGenPV(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(4);}
 228 CvBlobTrackFVGen* cvCreateFVGenPVS(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(5);}
 229 #undef MAX_FV_SIZE
 230
 231 #define MAX_FV_SIZE 4
 232 class CvBlobTrackFVGenSS: public CvBlobTrackFVGen
 233 {
 234 private:
 235     CvBlobSeq       m_BlobList;
 236     CvMemStorage*   m_pMem;
 237     CvSeq*          m_pFVSeq;
 238     float           m_FVMax[MAX_FV_SIZE];
 239     float           m_FVMin[MAX_FV_SIZE];
 240     float           m_FVVar[MAX_FV_SIZE];
 241     int             m_Dim;
 242     CvBlob          m_BlobSeq[BLOB_NUM];
 243     int             m_Frame;
 244     int             m_State;
 245     int             m_LastFrame;
 246     int             m_ClearFlag;
 247     void Clear()
 248     {
 249         cvClearMemStorage(m_pMem);
 250         m_pFVSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(float)*(m_Dim+1), m_pMem);
 251         m_ClearFlag = 1;
 252     }
 253 public:
 254     CvBlobTrackFVGenSS(int dim = 2 ):m_BlobList(sizeof(DefBlobFVN))
 255     {
 256         int i;
 257         assert(dim <= MAX_FV_SIZE);
 258         m_Dim = dim;
 259         for(i=0;i<m_Dim;++i)
 260         {
 261             m_FVVar[i] = 0.01f;
 262             m_FVMax[i] = 1;
 263             m_FVMin[i] = 0;
 264         }
 265         m_Frame = 0;
 266         m_State = 0;
 267         m_pMem = cvCreateMemStorage();
 268         m_pFVSeq = NULL;
 269     };
 270     ~CvBlobTrackFVGenSS()
 271     {
 272         if(m_pMem)cvReleaseMemStorage(&m_pMem);
 273     };
 274
 275     void AddBlob(CvBlob* pBlob)
 276     {
 277         //float       FV[MAX_FV_SIZE+1];
 278         int         i;
 279         DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
 280
 281         if(!m_ClearFlag) Clear();
 282
 283         if(pFVBlob==NULL)
 284         {
 285             DefBlobFVN BlobNew;
 286             BlobNew.blob = pBlob[0];
 287             BlobNew.LastFrame = m_Frame;
 288             BlobNew.state = 0;;
 289             BlobNew.FrameNum = 0;
 290             m_BlobList.AddBlob((CvBlob*)&BlobNew);
 291             pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
 292         }/* add new record if need */
 293
 294         /* shift */
 295         for(i=(BLOB_NUM-1);i>0;--i)
 296         {
 297             pFVBlob->BlobSeq[i] = pFVBlob->BlobSeq[i-1];
 298         }
 299
 300         pFVBlob->BlobSeq[0] = pBlob[0];
 301
 302         if(pFVBlob->FrameNum > BLOB_NUM)
 303         {/* average velocity */
 304             CvBlob* pBlobSeq = pFVBlob->BlobSeq;
 305             float   T = (CV_BLOB_WX(pBlob)+CV_BLOB_WY(pBlob))*0.01f;
 306             float   AverVx = 0;
 307             float   AverVy = 0;
 308             int     i;
 309             for(i=1;i<BLOB_NUM;++i)
 310             {
 311                 AverVx += CV_BLOB_X(pBlobSeq+i-1)-CV_BLOB_X(pBlobSeq+i);
 312                 AverVy += CV_BLOB_Y(pBlobSeq+i-1)-CV_BLOB_Y(pBlobSeq+i);
 313             }
 314             AverVx /= BLOB_NUM-1;
 315             AverVy /= BLOB_NUM-1;
 316
 317             if( fabs(AverVx) < T && fabs(AverVy) < T)
 318                 pFVBlob->state++;
 319             else
 320                 pFVBlob->state=0;
 321         }
 322
 323         if(pFVBlob->state == 5)
 324         { /* object is stoped  */
 325             float   FV[MAX_FV_SIZE];
 326             FV[0] = pFVBlob->blob.x;
 327             FV[1] = pFVBlob->blob.y;
 328             FV[2] = pFVBlob->BlobSeq[0].x;
 329             FV[3] = pFVBlob->BlobSeq[0].y;
 330             *(int*)(FV+m_Dim) = CV_BLOB_ID(pBlob);
 331             cvSeqPush( m_pFVSeq, FV );
 332         } /* object is stoped  */
 333
 334         pFVBlob->FrameNum++;
 335         pFVBlob->LastFrame = m_Frame;
 336     };  /* AddBlob */
 337     void Process(IplImage* pImg, IplImage* /*pFG*/)
 338     {
 339         int i;
 340
 341         if(!m_ClearFlag) Clear();
 342
 343         for(i=m_BlobList.GetBlobNum();i>0;--i)
 344         { /* delete unused blob */
 345             DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlob(i-1);
 346             if(pFVBlob->LastFrame < m_Frame)
 347             {
 348                 float   FV[MAX_FV_SIZE+1];
 349                 FV[0] = pFVBlob->blob.x;
 350                 FV[1] = pFVBlob->blob.y;
 351                 FV[2] = pFVBlob->BlobSeq[0].x;
 352                 FV[3] = pFVBlob->BlobSeq[0].y;
 353                 *(int*)(FV+m_Dim) = CV_BLOB_ID(pFVBlob);
 354                 cvSeqPush( m_pFVSeq, FV );
 355                 m_BlobList.DelBlob(i-1);
 356             }
 357         }/* check next blob in list */
 358
 359         /* set max min range */
 360         m_FVMin[0] = 0;
 361         m_FVMin[1] = 0;
 362         m_FVMin[2] = 0;
 363         m_FVMin[3] = 0;
 364         m_FVMax[0] = (float)(pImg->width-1);
 365         m_FVMax[1] = (float)(pImg->height-1);
 366         m_FVMax[2] = (float)(pImg->width-1);
 367         m_FVMax[3] = (float)(pImg->height-1);
 368         m_FVVar[0] = m_FVMax[0]*0.01f;
 369         m_FVVar[1] = m_FVMax[1]*0.01f;
 370         m_FVVar[2] = m_FVMax[2]*0.01f;
 371         m_FVVar[3] = m_FVMax[3]*0.01f;
 372
 373         m_Frame++;
 374         m_ClearFlag = 0;
 375     };
 376     virtual void    Release(){delete this;};
 377     virtual int     GetFVSize(){return m_Dim;};
 378     virtual int     GetFVNum()
 379     {
 380         return m_pFVSeq->total;
 381     };
 382     virtual float*  GetFV(int index, int* pFVID)
 383     {
 384         float* pFV = (float*)cvGetSeqElem( m_pFVSeq, index );
 385         if(pFVID)pFVID[0] = *(int*)(pFV+m_Dim);
 386         return pFV;
 387     };
 388     virtual float*  GetFVMin(){return m_FVMin;}; /* returned pointer to array of minimal values of FV, if return 0 then FVrange is not exist */
 389     virtual float*  GetFVMax(){return m_FVMax;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
 390     virtual float*  GetFVVar(){return m_FVVar;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
 391 };/* CvBlobTrackFVGenSS */
 392 CvBlobTrackFVGen* cvCreateFVGenSS(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenSS;}
 393
 394 /*======================= TRAJECTORY ANALAZER MODULES =====================*/
 395 /* Trajectory Analyser module */
 396 #define SPARSE  0
 397 #define ND      1
 398 #define BYSIZE  -1
 399 class DefMat
 400 {
 401 private:
 402     CvSparseMatIterator m_SparseIterator;
 403     CvSparseNode*       m_pSparseNode;
 404     int*                m_IDXs;
 405     int                 m_Dim;
 406
 407 public:
 408     CvSparseMat*        m_pSparse;
 409     CvMatND*            m_pND;
 410     int                 m_Volume;
 411     int                 m_Max;
 412     DefMat(int dim = 0, int* sizes = NULL, int type = SPARSE)
 413     {
 414         /* create sparse or ND matrix but not both */
 415         m_pSparseNode = NULL;
 416         m_pSparse = NULL;
 417         m_pND = NULL;
 418         m_Volume = 0;
 419         m_Max = 0;
 420         m_IDXs = NULL;
 421         m_Dim = 0;
 422         if(dim>0 && sizes != 0)
 423             Realloc(dim, sizes, type);
 424     }
 425     ~DefMat()
 426     {
 427         if(m_pSparse)cvReleaseSparseMat(&m_pSparse);
 428         if(m_pND)cvReleaseMatND(&m_pND);
 429         if(m_IDXs) cvFree(&m_IDXs);
 430     }
 431
 432     void Realloc(int dim, int* sizes, int type = SPARSE)
 433     {
 434         if(m_pSparse)cvReleaseSparseMat(&m_pSparse);
 435         if(m_pND)cvReleaseMatND(&m_pND);
 436
 437         if(type == BYSIZE )
 438         {
 439             int size = 0;
 440             int i;
 441             for(size=1,i=0;i<dim;++i)
 442             {
 443                 size *= sizes[i];
 444             }
 445             size *= sizeof(int);
 446             if(size > (2<<20))
 447             { /* if size > 1M */
 448                 type = SPARSE;
 449             }
 450             else
 451             {
 452                 type = ND;
 453             }
 454         }/* define matrix type */
 455
 456         if(type == SPARSE)
 457         {
 458             m_pSparse = cvCreateSparseMat( dim, sizes, CV_32SC1 );
 459             m_Dim = dim;
 460         }
 461         if(type == ND )
 462         {
 463             m_pND = cvCreateMatND( dim, sizes, CV_32SC1 );
 464             cvZero(m_pND);
 465             m_IDXs = (int*)cvAlloc(sizeof(int)*dim);
 466             m_Dim = dim;
 467         }
 468         m_Volume = 0;
 469         m_Max = 0;
 470     }
 471     void Save(char* File)
 472     {
 473         if(m_pSparse)cvSave(File, m_pSparse );
 474         if(m_pND)cvSave(File, m_pND );
 475     }
 476     void Save(CvFileStorage* fs, char* name)
 477     {
 478         if(m_pSparse)
 479         {
 480             cvWrite(fs, name, m_pSparse );
 481         }
 482         else if(m_pND)
 483         {
 484             cvWrite(fs, name, m_pND );
 485         }
 486     }
 487     void Load(char* File)
 488     {
 489         CvFileStorage* fs = cvOpenFileStorage( File, NULL, CV_STORAGE_READ );
 490         if(fs)
 491         {
 492             void* ptr;
 493             if(m_pSparse) cvReleaseSparseMat(&m_pSparse);
 494             if(m_pND) cvReleaseMatND(&m_pND);
 495             m_Volume = 0;
 496             m_Max = 0;
 497             ptr = cvLoad(File);
 498             if(ptr && CV_IS_MATND_HDR(ptr)) m_pND = (CvMatND*)ptr;
 499             if(ptr && CV_IS_SPARSE_MAT_HDR(ptr)) m_pSparse = (CvSparseMat*)ptr;
 500             cvReleaseFileStorage(&fs);
 501         }
 502         AfterLoad();
 503     }/* Load */
 504
 505     void Load(CvFileStorage* fs, CvFileNode* node, char* name)
 506     {
 507         CvFileNode* n = cvGetFileNodeByName(fs,node,name);
 508         void* ptr = n?cvRead(fs,n):NULL;
 509         if(ptr)
 510         {
 511             if(m_pSparse) cvReleaseSparseMat(&m_pSparse);
 512             if(m_pND) cvReleaseMatND(&m_pND);
 513             m_Volume = 0;
 514             m_Max = 0;
 515             if(CV_IS_MATND_HDR(ptr)) m_pND = (CvMatND*)ptr;
 516             if(CV_IS_SPARSE_MAT_HDR(ptr)) m_pSparse = (CvSparseMat*)ptr;
 517         }
 518         else
 519         {
 520             printf("WARNING!!! Can't load %s matrix\n",name);
 521         }
 522         AfterLoad();
 523     }/* Load */
 524     void AfterLoad()
 525     {
 526         m_Volume = 0;
 527         m_Max = 0;
 528         if(m_pSparse)
 529         {/* calculate Volume of loaded hist */
 530             CvSparseMatIterator mat_iterator;
 531             CvSparseNode* node = cvInitSparseMatIterator( m_pSparse, &mat_iterator );
 532
 533             for( ; node != 0; node = cvGetNextSparseNode( &mat_iterator ))
 534             {
 535                 int val = *(int*)CV_NODE_VAL( m_pSparse, node ); /* get value of the element
 536                                                                 (assume that the type is CV_32SC1) */
 537                 m_Volume += val;
 538                 if(m_Max < val)m_Max = val;
 539             }
 540         }/* calculate Volume of loaded hist */
 541         if(m_pND)
 542         {/* calculate Volume of loaded hist */
 543             CvMat   mat;
 544             double  max_val;
 545             double  vol;
 546             cvGetMat( m_pND, &mat, NULL, 1 );
 547
 548             vol = cvSum(&mat).val[0];
 549             m_Volume = cvRound(vol);
 550             cvMinMaxLoc( &mat, NULL, &max_val);
 551             m_Max = cvRound(max_val);
 552             /* MUST BE WRITTEN LATER */
 553         }/* calculate Volume of loaded hist */
 554     }/* AfterLoad */
 555     int* GetPtr(int* indx)
 556     {
 557         if(m_pSparse) return (int*)cvPtrND( m_pSparse, indx, NULL, 1, NULL);
 558         if(m_pND) return  (int*)cvPtrND( m_pND, indx, NULL, 1, NULL);
 559         return NULL;
 560     }/* GetPtr */
 561     int GetVal(int* indx)
 562     {
 563         int* p = GetPtr(indx);
 564         if(p)return p[0];
 565         return -1;
 566     }/* GetVal */
 567     int Add(int* indx, int val)
 568     {
 569         int  NewVal;
 570         int* pVal = GetPtr(indx);
 571         if(pVal == NULL) return -1;
 572         pVal[0] += val;
 573         NewVal = pVal[0];
 574         m_Volume += val;
 575         if(m_Max < NewVal)m_Max = NewVal;
 576         return NewVal;
 577     }/* Add */
 578     void Add(DefMat* pMatAdd)
 579     {
 580         int*    pIDXS = NULL;
 581         int     Val = 0;
 582         for(Val = pMatAdd->GetNext(&pIDXS, 1 );pIDXS;Val=pMatAdd->GetNext(&pIDXS, 0 ))
 583         {
 584             Add(pIDXS,Val);
 585         }
 586     }/* Add */
 587     int SetMax(int* indx, int val)
 588     {
 589         int  NewVal;
 590         int* pVal = GetPtr(indx);
 591         if(pVal == NULL) return -1;
 592         if(val > pVal[0])
 593         {
 594             m_Volume += val-pVal[0];
 595             pVal[0] = val;
 596         }
 597         NewVal = pVal[0];
 598         if(m_Max < NewVal)m_Max = NewVal;
 599         return NewVal;
 600     }/* Add */
 601     int GetNext(int** pIDXS, int init = 0)
 602     {
 603         int     Val = 0;
 604         pIDXS[0] = NULL;
 605         if(m_pSparse)
 606         {
 607             m_pSparseNode = (init || m_pSparseNode==NULL)?
 608                 cvInitSparseMatIterator( m_pSparse, &m_SparseIterator ):
 609                 cvGetNextSparseNode( &m_SparseIterator );
 610
 611             if(m_pSparseNode)
 612             {
 613                 int* pVal = (int*)CV_NODE_VAL( m_pSparse, m_pSparseNode );
 614                 if(pVal)Val = pVal[0];
 615                 pIDXS[0] = CV_NODE_IDX( m_pSparse, m_pSparseNode );
 616             }
 617         }/* sparce matrix */
 618         if(m_pND)
 619         {
 620             int i;
 621             if(init)
 622             {
 623                 for(i=0;i<m_Dim;++i)
 624                 {
 625                     m_IDXs[i] = cvGetDimSize( m_pND, i )-1;
 626                 }
 627                 pIDXS[0] = m_IDXs;
 628                 Val = GetVal(m_IDXs);
 629             }
 630             else
 631             {
 632                 for(i=0;i<m_Dim;++i)
 633                 {
 634                     if((m_IDXs[i]--)>0)
 635                         break;
 636                     m_IDXs[i] = cvGetDimSize( m_pND, i )-1;
 637                 }
 638                 if(i==m_Dim)
 639                 {
 640                     pIDXS[0] = NULL;
 641                 }
 642                 else
 643                 {
 644                     pIDXS[0] = m_IDXs;
 645                     Val = GetVal(m_IDXs);
 646                 }
 647             }/* get next ND */
 648         }/* sparce matrix */
 649         return Val;
 650     };/* GetNext */
 651 };
 652
 653 #define FV_NUM 10
 654 #define FV_SIZE 10
 655 typedef struct DefTrackFG
 656 {
 657     CvBlob                  blob;
 658 //    CvBlobTrackFVGen*       pFVGen;
 659     int                     LastFrame;
 660     float                   state;
 661     DefMat*                 pHist;
 662 } DefTrackFG;
 663 class CvBlobTrackAnalysisHist : public CvBlobTrackAnalysis
 664 {
 665     /*---------------- internal functions --------------------*/
 666 private:
 667     int                 m_BinNumParam;
 668     int                 m_SmoothRadius;
 669     char*               m_SmoothKernel;
 670     float               m_AbnormalThreshold;
 671     int                 m_TrackNum;
 672     int                 m_Frame;
 673     int                 m_BinNum;
 674     char                m_DataFileName[1024];
 675     int                 m_Dim;
 676     int*                m_Sizes;
 677     DefMat              m_HistMat;
 678     int                 m_HistVolumeSaved;
 679     int*                m_pFVi;
 680     int*                m_pFViVar;
 681     int*                m_pFViVarRes;
 682     CvBlobSeq           m_TrackFGList;
 683     //CvBlobTrackFVGen*   (*m_CreateFVGen)();
 684     CvBlobTrackFVGen*   m_pFVGen;
 685     void SaveHist()
 686     {
 687         if(m_DataFileName[0])
 688         {
 689             m_HistMat.Save(m_DataFileName);
 690             m_HistVolumeSaved = m_HistMat.m_Volume;
 691         }
 692     };
 693     void LoadHist()
 694     {
 695         if(m_DataFileName[0])m_HistMat.Load(m_DataFileName);
 696         m_HistVolumeSaved = m_HistMat.m_Volume;
 697     }
 698     void AllocData()
 699     {/* AllocData */
 700         m_pFVi = (int*)cvAlloc(sizeof(int)*m_Dim);
 701         m_pFViVar = (int*)cvAlloc(sizeof(int)*m_Dim);
 702         m_pFViVarRes = (int*)cvAlloc(sizeof(int)*m_Dim);
 703         m_Sizes = (int*)cvAlloc(sizeof(int)*m_Dim);
 704
 705         { /* create init sparce matrix */
 706             int     i;
 707             for(i=0;i<m_Dim;++i)m_Sizes[i] = m_BinNum;
 708             m_HistMat.Realloc(m_Dim,m_Sizes,SPARSE);
 709             m_HistVolumeSaved = 0;
 710         } /* create init sparce matrix */
 711     }/* AllocData */
 712     void FreeData()
 713     { /* FreeData */
 714         int i;
 715         for(i=m_TrackFGList.GetBlobNum();i>0;--i)
 716         {
 717             //DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i-1);
 718 //            pF->pFVGen->Release();
 719             m_TrackFGList.DelBlob(i-1);
 720         }
 721         cvFree(&m_pFVi);
 722         cvFree(&m_pFViVar);
 723         cvFree(&m_pFViVarRes);
 724         cvFree(&m_Sizes);
 725     }/* FreeData */
 726     virtual void ParamUpdate()
 727     {
 728         if(m_BinNum != m_BinNumParam)
 729         {
 730             FreeData();
 731             m_BinNum = m_BinNumParam;
 732             AllocData();
 733         }
 734     }
 735 public:
 736     CvBlobTrackAnalysisHist(CvBlobTrackFVGen*   (*createFVGen)()):m_TrackFGList(sizeof(DefTrackFG))
 737     {
 738         m_pFVGen = createFVGen();
 739         m_Dim = m_pFVGen->GetFVSize();
 740         m_Frame = 0;
 741         m_pFVi = 0;
 742         m_TrackNum = 0;
 743         m_BinNum = 32;
 744         m_DataFileName[0] = 0;
 745
 746         m_AbnormalThreshold = 0.02f;
 747         AddParam("AbnormalThreshold",&m_AbnormalThreshold);
 748         CommentParam("AbnormalThreshold","If trajectory histogram value is lesst then <AbnormalThreshold*DataBaseTrackNum> then trajectory is abnormal");
 749
 750         m_SmoothRadius = 1;
 751         AddParam("SmoothRadius",&m_SmoothRadius);
 752         CommentParam("AbnormalThreshold","Radius (in bins) for histogramm smothing");
 753
 754         m_SmoothKernel = "L";
 755         AddParam("SmoothKernel",&m_SmoothKernel);
 756         CommentParam("SmoothKernel","L - Linear, G - Gaussian");
 757
 758
 759         m_BinNumParam = m_BinNum;
 760         AddParam("BinNum",&m_BinNumParam);
 761         CommentParam("BinNum","Number of bin for each dimention of feature vector");
 762
 763         AllocData();
 764     }/* constructor */
 765     ~CvBlobTrackAnalysisHist()
 766     {
 767         SaveHist();
 768         FreeData();
 769         m_pFVGen->Release();
 770     }/* destructor */
 771
 772     /*-----------------  interface --------------------*/
 773     virtual void    AddBlob(CvBlob* pBlob)
 774     {
 775         DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(CV_BLOB_ID(pBlob));
 776         if(pF == NULL)
 777         { /* create new filter */
 778             DefTrackFG F;
 779             F.state = 0;
 780             F.blob = pBlob[0];
 781             F.LastFrame = m_Frame;
 782 //            F.pFVGen = m_CreateFVGen();
 783             F.pHist = new DefMat(m_Dim,m_Sizes,SPARSE);
 784             m_TrackFGList.AddBlob((CvBlob*)&F);
 785             pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(CV_BLOB_ID(pBlob));
 786         }
 787
 788         assert(pF);
 789         pF->blob = pBlob[0];
 790         pF->LastFrame = m_Frame;
 791         m_pFVGen->AddBlob(pBlob);
 792     };
 793     virtual void Process(IplImage* pImg, IplImage* pFG)
 794     {
 795         int i;
 796         m_pFVGen->Process(pImg, pFG);
 797         int SK = m_SmoothKernel[0];
 798
 799         for(i=0;i<m_pFVGen->GetFVNum();++i)
 800         {
 801             int         BlobID = 0;
 802             float*      pFV = m_pFVGen->GetFV(i,&BlobID);
 803             float*      pFVMax = m_pFVGen->GetFVMax();
 804             float*      pFVMin = m_pFVGen->GetFVMin();
 805             DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(BlobID);
 806             int         HistVal = 1;
 807
 808             if(pFV==NULL) break;
 809
 810             pF->LastFrame = m_Frame;
 811
 812             {/* binarize FV */
 813                 int         j;
 814                 for(j=0;j<m_Dim;++j)
 815                 {
 816                     int     index;
 817                     float   f0 = pFVMin?pFVMin[j]:0;
 818                     float   f1 = pFVMax?pFVMax[j]:1;
 819                     assert(f1>f0);
 820                     index = cvRound((m_BinNum-1)*(pFV[j]-f0)/(f1-f0));
 821                     if(index<0)index=0;
 822                     if(index>=m_BinNum)index=m_BinNum-1;
 823                     m_pFVi[j] = index;
 824                 }
 825             }
 826
 827             HistVal = m_HistMat.GetVal(m_pFVi);/* get bin value*/
 828             pF->state = 0;
 829             { /* calc state */
 830                 float   T = m_HistMat.m_Max*m_AbnormalThreshold; /* calc threshold */
 831
 832                 if(m_TrackNum>0) T = 256.0f * m_TrackNum*m_AbnormalThreshold;
 833                 if(T>0)
 834                 {
 835                     pF->state = (T - HistVal)/(T*0.2f) + 0.5f;
 836                 }
 837                 if(pF->state<0)pF->state=0;
 838                 if(pF->state>1)pF->state=1;
 839             }
 840
 841             {/* if new FV then add it to trajectory histogramm */
 842                 int i,flag = 1;
 843                 int r = m_SmoothRadius;
 844
 845 //                    printf("BLob %3d NEW FV [", CV_BLOB_ID(pF));
 846 //                    for(i=0;i<m_Dim;++i) printf("%d,", m_pFVi[i]);
 847 //                    printf("]");
 848
 849                 for(i=0;i<m_Dim;++i)
 850                 {
 851                     m_pFViVar[i]=-r;
 852                 }
 853                 while(flag)
 854                 {
 855                     float   dist = 0;
 856                     int     HistAdd = 0;
 857                     int     i;
 858                     int     good = 1;
 859                     for(i=0;i<m_Dim;++i)
 860                     {
 861                         m_pFViVarRes[i] = m_pFVi[i]+m_pFViVar[i];
 862                         if(m_pFViVarRes[i]<0) good= 0;
 863                         if(m_pFViVarRes[i]>=m_BinNum) good= 0;
 864                         dist += m_pFViVar[i]*m_pFViVar[i];
 865                     }/* calc next dimention */
 866
 867                     if(SK=='G' || SK=='g')
 868                     {
 869                         double dist2 = dist/(r*r);
 870                         HistAdd = cvRound(256*exp(-dist2)); /* Hist Add for (dist=1) = 25.6*/
 871                     }
 872                     else if(SK=='L' || SK=='l')
 873                     {
 874                         dist = (float)(sqrt(dist)/(r+1));
 875                         HistAdd = cvRound(256*(1-dist));
 876                     }
 877                     else
 878                     {
 879                         HistAdd = 255; /* flat smothing */
 880                     }
 881
 882                     if(good && HistAdd>0)
 883                     {/* update hist */
 884                         assert(pF->pHist);
 885                         pF->pHist->SetMax(m_pFViVarRes, HistAdd);
 886                     }/* update hist */
 887
 888                     for(i=0;i<m_Dim;++i)
 889                     {/* next config */
 890                         if((m_pFViVar[i]++) < r)
 891                             break;
 892                         m_pFViVar[i] = -r;
 893                     }/* increase next dim var*/
 894                     if(i==m_Dim)break;
 895                 }/* next variation */
 896             }/* if new FV */
 897         }/* next FV */
 898
 899         {/* check all blob from list */
 900             int i;
 901             for(i=m_TrackFGList.GetBlobNum();i>0;--i)
 902             {/* add histogramm and delete blob from list */
 903                 DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i-1);
 904                 if(pF->LastFrame+3 < m_Frame && pF->pHist)
 905                 {
 906                     m_HistMat.Add(pF->pHist);
 907                     delete pF->pHist;
 908                     m_TrackNum++;
 909                     m_TrackFGList.DelBlob(i-1);
 910                 }
 911             }/* next blob */
 912         }
 913
 914         m_Frame++;
 915
 916         if(m_Wnd)
 917         {/* debug output */
 918             int*        idxs = NULL;
 919             int         Val = 0;
 920             IplImage*   pI = cvCloneImage(pImg);
 921
 922             cvZero(pI);
 923             for(Val = m_HistMat.GetNext(&idxs,1);idxs;Val=m_HistMat.GetNext(&idxs,0))
 924             {/* draw all elements */
 925                 float   vf;
 926                 int     x,y;
 927
 928                 if(!idxs) break;
 929                 if(Val == 0) continue;
 930
 931                 vf = (float)Val/(m_HistMat.m_Max?m_HistMat.m_Max:1);
 932                 x = cvRound((float)(pI->width-1)*(float)idxs[0] / (float)m_BinNum);
 933                 y = cvRound((float)(pI->height-1)*(float)idxs[1] / (float)m_BinNum);
 934
 935                 cvCircle(pI, cvPoint(x,y), cvRound(vf*pI->height/(m_BinNum*2)),CV_RGB(255,0,0),CV_FILLED);
 936                 if(m_Dim > 3)
 937                 {
 938                     int dx = -2*(idxs[2]-m_BinNum/2);
 939                     int dy = -2*(idxs[3]-m_BinNum/2);
 940                     cvLine(pI,cvPoint(x,y),cvPoint(x+dx,y+dy),CV_RGB(0,cvRound(vf*255),1));
 941                 }
 942                 if( m_Dim==4 &&
 943                     m_pFVGen->GetFVMax()[0]==m_pFVGen->GetFVMax()[2] &&
 944                     m_pFVGen->GetFVMax()[1]==m_pFVGen->GetFVMax()[3])
 945                 {
 946                     int x = cvRound((float)(pI->width-1)*(float)idxs[2] / (float)m_BinNum);
 947                     int y = cvRound((float)(pI->height-1)*(float)idxs[3] / (float)m_BinNum);
 948                     cvCircle(pI, cvPoint(x,y), cvRound(vf*pI->height/(m_BinNum*2)),CV_RGB(0,0,255),CV_FILLED);
 949                 }
 950             }/* draw all elements  */
 951
 952             for(i=m_TrackFGList.GetBlobNum();i>0;--i)
 953             {
 954                 DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i-1);
 955                 DefMat* pHist = pF?pF->pHist:NULL;
 956
 957                 if(pHist==NULL) continue;
 958
 959                 for(Val = pHist->GetNext(&idxs,1);idxs;Val=pHist->GetNext(&idxs,0))
 960                 {/* draw all elements */
 961                     float   vf;
 962                     int     x,y;
 963
 964                     if(!idxs) break;
 965                     if(Val == 0) continue;
 966
 967                     vf = (float)Val/(pHist->m_Max?pHist->m_Max:1);
 968                     x = cvRound((float)(pI->width-1)*(float)idxs[0] / (float)m_BinNum);
 969                     y = cvRound((float)(pI->height-1)*(float)idxs[1] / (float)m_BinNum);
 970
 971                     cvCircle(pI, cvPoint(x,y), cvRound(2*vf),CV_RGB(0,0,cvRound(255*vf)),CV_FILLED);
 972                     if(m_Dim > 3)
 973                     {
 974                         int dx = -2*(idxs[2]-m_BinNum/2);
 975                         int dy = -2*(idxs[3]-m_BinNum/2);
 976                         cvLine(pI,cvPoint(x,y),cvPoint(x+dx,y+dy),CV_RGB(0,0,255));
 977                     }
 978                     if( m_Dim==4 &&
 979                         m_pFVGen->GetFVMax()[0]==m_pFVGen->GetFVMax()[2] &&
 980                         m_pFVGen->GetFVMax()[1]==m_pFVGen->GetFVMax()[3])
 981                     { /* if SS feature vector */
 982                         int x = cvRound((float)(pI->width-1)*(float)idxs[2] / (float)m_BinNum);
 983                         int y = cvRound((float)(pI->height-1)*(float)idxs[3] / (float)m_BinNum);
 984                         cvCircle(pI, cvPoint(x,y), cvRound(vf*pI->height/(m_BinNum*2)),CV_RGB(0,0,255),CV_FILLED);
 985                     }
 986                 }/* draw all elements  */
 987             }/* next track */
 988
 989             //cvNamedWindow("Hist",0);
 990             //cvShowImage("Hist", pI);
 991             cvReleaseImage(&pI);
 992         }
 993     };
 994     float GetState(int BlobID)
 995     {
 996         DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(BlobID);
 997         return pF?pF->state:0.0f;
 998     };
 999     /* return 0 if trajectory is normal
1000        return >0 if trajectory abnormal */
1001     virtual char*   GetStateDesc(int BlobID)
1002     {
1003         if(GetState(BlobID)>0.5) return "abnormal";
1004         return NULL;
1005     }
1006     virtual void    SetFileName(char* DataBaseName)
1007     {
1008         if(m_HistMat.m_Volume!=m_HistVolumeSaved)SaveHist();
1009         m_DataFileName[0] = 0;
1010         if(DataBaseName)
1011         {
1012             strncpy(m_DataFileName,DataBaseName,1000);
1013             strcat(m_DataFileName, ".yml");
1014         }
1015         LoadHist();
1016     };
1017     virtual void SaveState(CvFileStorage* fs)
1018     {
1019         int b, bN = m_TrackFGList.GetBlobNum();
1020         cvWriteInt(fs,"BlobNum",bN);
1021         cvStartWriteStruct(fs,"BlobList",CV_NODE_SEQ);
1022         for(b=0;b<bN;++b)
1023         {
1024             DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(b);
1025             cvStartWriteStruct(fs,NULL,CV_NODE_MAP);
1026             cvWriteStruct(fs,"Blob", &(pF->blob), "ffffi");
1027             cvWriteInt(fs,"LastFrame",pF->LastFrame);
1028             cvWriteReal(fs,"State",pF->state);
1029             pF->pHist->Save(fs, "Hist");
1030             cvEndWriteStruct(fs);
1031         }
1032         cvEndWriteStruct(fs);
1033         m_HistMat.Save(fs, "Hist");
1034     };
1035     virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
1036     {
1037         CvFileNode* pBLN = cvGetFileNodeByName(fs,node,"BlobList");
1038
1039         if(pBLN && CV_NODE_IS_SEQ(pBLN->tag))
1040         {
1041             int b, bN = pBLN->data.seq->total;
1042             for(b=0;b<bN;++b)
1043             {
1044                 DefTrackFG* pF = NULL;
1045                 CvBlob      Blob;
1046                 CvFileNode* pBN = (CvFileNode*)cvGetSeqElem(pBLN->data.seq,b);
1047
1048                 assert(pBN);
1049                 cvReadStructByName(fs, pBN, "Blob", &Blob, "ffffi");
1050                 AddBlob(&Blob);
1051                 pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(Blob.ID);
1052                 if(pF==NULL) continue;
1053                 assert(pF);
1054                 pF->state = (float)cvReadIntByName(fs,pBN,"State",cvRound(pF->state));
1055                 assert(pF->pHist);
1056                 pF->pHist->Load(fs,pBN,"Hist");
1057             }
1058         }
1059
1060         m_HistMat.Load(fs, node, "Hist");
1061     }; /* LoadState */
1062
1063
1064     virtual void    Release(){ delete this; };
1065
1066 };
1067
1068
1069
1070 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistP()
1071 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenP);}
1072 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistPV()
1073 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenPV);}
1074 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistPVS()
1075 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenPVS);}
1076 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistSS()
1077 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenSS);}
1078
1079 typedef struct DefTrackSVM
1080 {
1081     CvBlob                  blob;
1082 //    CvBlobTrackFVGen*       pFVGen;
1083     int                     LastFrame;
1084     float                   state;
1085     CvBlob                  BlobLast;
1086     CvSeq*                  pFVSeq;
1087     CvMemStorage*           pMem;
1088 } DefTrackSVM;
1089
1090 class CvBlobTrackAnalysisSVM : public CvBlobTrackAnalysis
1091 {
1092     /*---------------- internal functions --------------------*/
1093 private:
1094     CvMemStorage*       m_pMem;
1095     int                 m_TrackNum;
1096     int                 m_Frame;
1097     char                m_DataFileName[1024];
1098     int                 m_Dim;
1099     float*              m_pFV;
1100     //CvStatModel*        m_pStatModel;
1101     void*               m_pStatModel;
1102     CvBlobSeq           m_Tracks;
1103     CvMat*              m_pTrainData;
1104     int                 m_LastTrainDataSize;
1105 //    CvBlobTrackFVGen*   (*m_CreateFVGen)();
1106     CvBlobTrackFVGen*   m_pFVGen;
1107     float               m_NU;
1108     float               m_RBFWidth;
1109     IplImage*           m_pStatImg; /* for debug purpose */
1110     CvSize              m_ImgSize;
1111     void RetrainStatModel()
1112     {
1113 ///////// !!!!! TODO !!!!! Repair /////////////
1114 #if 0
1115         float               nu = 0;
1116         CvSVMModelParams    SVMParams = {0};
1117         CvStatModel*        pM = NULL;
1118
1119
1120         memset(&SVMParams,0,sizeof(SVMParams));
1121         SVMParams.svm_type = CV_SVM_ONE_CLASS;
1122         SVMParams.kernel_type = CV_SVM_RBF;
1123         SVMParams.gamma = 2.0/(m_RBFWidth*m_RBFWidth);
1124         SVMParams.nu = m_NU;
1125         SVMParams.degree = 3;
1126         SVMParams.criteria = cvTermCriteria(CV_TERMCRIT_EPS, 100, 1e-3 );
1127         SVMParams.C = 1;
1128         SVMParams.p = 0.1;
1129
1130
1131         if(m_pTrainData == NULL) return;
1132         {
1133             int64       TickCount = cvGetTickCount();
1134             printf("Frame: %d\n           Retrain SVM\nData Size = %d\n",m_Frame, m_pTrainData->rows);
1135             pM = cvTrainSVM( m_pTrainData,CV_ROW_SAMPLE, NULL, (CvStatModelParams*)&SVMParams, NULL, NULL);
1136             TickCount = cvGetTickCount() - TickCount ;
1137             printf("SV Count = %d\n",((CvSVMModel*)pM)->sv_total);
1138             printf("Processing Time = %.1f(ms)\n",TickCount/(1000*cvGetTickFrequency()));
1139
1140         }
1141         if(pM==NULL) return;
1142         if(m_pStatModel) cvReleaseStatModel(&m_pStatModel);
1143         m_pStatModel = pM;
1144
1145         if(m_pTrainData && m_Wnd)
1146         {
1147             float       MaxVal = 0;
1148             IplImage*   pW = cvCreateImage(m_ImgSize,IPL_DEPTH_32F,1);
1149             IplImage*   pI = cvCreateImage(m_ImgSize,IPL_DEPTH_8U,1);
1150             float*      pFVVar = m_pFVGen->GetFVVar();
1151             int     i;
1152             cvZero(pW);
1153             for(i=0;i<m_pTrainData->rows;++i)
1154             {/* draw all elements */
1155                 float*          pFV = (float*)(m_pTrainData->data.ptr + m_pTrainData->step*i);
1156                 int             x = cvRound(pFV[0]*pFVVar[0]);
1157                 int             y = cvRound(pFV[1]*pFVVar[1]);
1158                 float           r;
1159
1160                 if(x<0)x=0;
1161                 if(x>=pW->width)x=pW->width-1;
1162                 if(y<0)y=0;
1163                 if(y>=pW->height)y=pW->height-1;
1164
1165                 r = ((float*)(pW->imageData + y*pW->widthStep))[x];
1166                 r++;
1167                 ((float*)(pW->imageData + y*pW->widthStep))[x] = r;
1168
1169                 if(r>MaxVal)MaxVal=r;
1170             }/* next point */
1171             if(MaxVal>0)cvConvertScale(pW,pI,255/MaxVal,0);
1172             cvNamedWindow("SVMData",0);
1173             cvShowImage("SVMData",pI);
1174             cvSaveImage("SVMData.bmp",pI);
1175             cvReleaseImage(&pW);
1176             cvReleaseImage(&pI);
1177         }/* prepare for debug */
1178
1179         if(m_pStatModel && m_Wnd && m_Dim == 2)
1180         {
1181             float*      pFVVar = m_pFVGen->GetFVVar();
1182             int x,y;
1183             if(m_pStatImg==NULL)
1184             {
1185                 m_pStatImg = cvCreateImage(m_ImgSize,IPL_DEPTH_8U,1);
1186             }
1187             cvZero(m_pStatImg);
1188             for(y=0;y<m_pStatImg->height;y+=1)for(x=0;x<m_pStatImg->width;x+=1)
1189             {/* draw all elements */
1190                 float           res;
1191                 uchar*  pData = (uchar*)m_pStatImg->imageData + x + y*m_pStatImg->widthStep;
1192                 CvMat           FVmat;
1193                 float           xy[2] = {x/pFVVar[0],y/pFVVar[1]};
1194                 cvInitMatHeader( &FVmat, 1, 2, CV_32F, xy );
1195                 res = cvStatModelPredict( m_pStatModel, &FVmat, NULL );
1196                 pData[0]=((res>0.5)?255:0);
1197             }/* next point */
1198             cvNamedWindow("SVMMask",0);
1199             cvShowImage("SVMMask",m_pStatImg);
1200             cvSaveImage("SVMMask.bmp",m_pStatImg);
1201         }/* prepare for debug */
1202 #endif
1203     };
1204     void SaveStatModel()
1205     {
1206         if(m_DataFileName[0])
1207         {
1208             if(m_pTrainData)cvSave(m_DataFileName, m_pTrainData);
1209         }
1210     };
1211     void LoadStatModel()
1212     {
1213         if(m_DataFileName[0])
1214         {
1215             CvMat* pTrainData = (CvMat*)cvLoad(m_DataFileName);
1216             if(CV_IS_MAT(pTrainData) && pTrainData->width == m_Dim)
1217             {
1218                 if(m_pTrainData) cvReleaseMat(&m_pTrainData);
1219                 m_pTrainData = pTrainData;
1220                 RetrainStatModel();
1221             }
1222         }
1223     }
1224 public:
1225     CvBlobTrackAnalysisSVM(CvBlobTrackFVGen*   (*createFVGen)()):m_Tracks(sizeof(DefTrackSVM))
1226     {
1227         m_pFVGen = createFVGen();
1228         m_Dim = m_pFVGen->GetFVSize();
1229         m_pFV = (float*)cvAlloc(sizeof(float)*m_Dim);
1230         m_Frame = 0;
1231         m_TrackNum = 0;
1232         m_pTrainData = NULL;
1233         m_pStatModel = NULL;
1234         m_DataFileName[0] = 0;
1235         m_pStatImg = NULL;
1236         m_LastTrainDataSize = 0;
1237
1238         m_NU = 0.2f;
1239         AddParam("Nu",&m_NU);
1240         CommentParam("Nu","Parameters that tunes SVM border elastic");
1241
1242         m_RBFWidth = 1;
1243         AddParam("RBFWidth",&m_RBFWidth);
1244         CommentParam("RBFWidth","Parameters that tunes RBF kernel function width.");
1245
1246     }/* constructor */
1247     ~CvBlobTrackAnalysisSVM()
1248     {
1249         int i;
1250         SaveStatModel();
1251         for(i=m_Tracks.GetBlobNum();i>0;--i)
1252         {
1253             DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlob(i-1);
1254             if(pF->pMem) cvReleaseMemStorage(&pF->pMem);
1255             //pF->pFVGen->Release();
1256         }
1257         if(m_pStatImg)cvReleaseImage(&m_pStatImg);
1258         cvFree(&m_pFV);
1259     }/* destructor */
1260
1261     /*-----------------  interface --------------------*/
1262     virtual void    AddBlob(CvBlob* pBlob)
1263     {
1264         DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
1265
1266         m_pFVGen->AddBlob(pBlob);
1267
1268         if(pF == NULL)
1269         { /* create new record */
1270             DefTrackSVM F;
1271             F.state = 0;
1272             F.blob = pBlob[0];
1273             F.LastFrame = m_Frame;
1274             //F.pFVGen = m_CreateFVGen();
1275             F.pMem = cvCreateMemStorage();
1276             F.pFVSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(float)*m_Dim,F.pMem);
1277
1278             F.BlobLast.x = -1;
1279             F.BlobLast.y = -1;
1280             F.BlobLast.w = -1;
1281             F.BlobLast.h = -1;
1282             m_Tracks.AddBlob((CvBlob*)&F);
1283             pF = (DefTrackSVM*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
1284         }
1285
1286         assert(pF);
1287         pF->blob = pBlob[0];
1288         pF->LastFrame = m_Frame;
1289     };
1290     virtual void Process(IplImage* pImg, IplImage* pFG)
1291     {
1292         int     i;
1293         float*  pFVVar = m_pFVGen->GetFVVar();
1294
1295         m_pFVGen->Process(pImg, pFG);
1296         m_ImgSize = cvSize(pImg->width,pImg->height);
1297
1298         for(i=m_pFVGen->GetFVNum();i>0;--i)
1299         {
1300             int             BlobID = 0;
1301             float*          pFV = m_pFVGen->GetFV(i,&BlobID);
1302             DefTrackSVM*    pF = (DefTrackSVM*)m_Tracks.GetBlobByID(BlobID);
1303             if(pF && pFV)
1304             {/* process */
1305                 float   dx,dy;
1306                 CvMat   FVmat;
1307
1308                 pF->state = 0;
1309
1310                 if(m_pStatModel)
1311                 {
1312                     int j;
1313                     for(j=0;j<m_Dim;++j)
1314                     {
1315                         m_pFV[j] = pFV[j]/pFVVar[j];
1316                     }
1317                     cvInitMatHeader( &FVmat, 1, m_Dim, CV_32F, m_pFV );
1318                     //pF->state = cvStatModelPredict( m_pStatModel, &FVmat, NULL )<0.5;
1319                     pF->state = 1.f;
1320                 }
1321
1322                 dx = (pF->blob.x - pF->BlobLast.x);
1323                 dy = (pF->blob.y - pF->BlobLast.y);
1324
1325                 if(pF->BlobLast.x<0 || (dx*dx+dy*dy) >= 2*2)
1326                 {   /* add feature vector to train data base */
1327                     pF->BlobLast = pF->blob;
1328                     cvSeqPush(pF->pFVSeq,pFV);
1329                 }
1330             }/* process one blob */
1331         }/* next FV */
1332
1333         for(i=m_Tracks.GetBlobNum();i>0;--i)
1334         {/* check each blob record */
1335             DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlob(i-1);
1336             if(pF->LastFrame+3 < m_Frame )
1337             {/* retrain stat model and delete blob filter */
1338                 int                 mult = 1+m_Dim;
1339                 int                 old_height = m_pTrainData?m_pTrainData->height:0;
1340                 int                 height = old_height + pF->pFVSeq->total*mult;
1341                 CvMat*              pTrainData = cvCreateMat(height, m_Dim, CV_32F);
1342                 int                 j;
1343                 if(m_pTrainData && pTrainData)
1344                 { /* create new train data matrix */
1345                     int h = pTrainData->height;
1346                     pTrainData->height = MIN(pTrainData->height, m_pTrainData->height);
1347                     cvCopy(m_pTrainData,pTrainData);
1348                     pTrainData->height = h;
1349                 }
1350                 for(j=0;j<pF->pFVSeq->total;++j)
1351                 {/* copy new data to train data */
1352                     float*  pFVVar = m_pFVGen->GetFVVar();
1353                     float*  pFV = (float*)cvGetSeqElem(pF->pFVSeq,j);
1354                     int     k;
1355                     for(k=0;k<mult;++k)
1356                     {
1357                         int t;
1358                         float*  pTD = (float*)CV_MAT_ELEM_PTR( pTrainData[0], old_height+j*mult+k, 0);
1359                         memcpy(pTD,pFV,sizeof(float)*m_Dim);
1360
1361                         if(pFVVar)for(t=0;t<m_Dim;++t)
1362                         {/* scale FV */
1363                             pTD[t] /= pFVVar[t];
1364                         }
1365
1366                         if(k>0)
1367                         {/* variate */
1368                             for(t=0;t<m_Dim;++t)
1369                             {
1370                                 pTD[t] += m_RBFWidth*0.5f*(1-2.0f*rand()/(float)RAND_MAX);
1371                             }
1372                         }
1373                     }
1374                 }/* next new data */
1375
1376                 if(m_pTrainData) cvReleaseMat(&m_pTrainData);
1377                 m_pTrainData = pTrainData;
1378
1379                 /* delete track record */
1380                 cvReleaseMemStorage(&pF->pMem);
1381                 m_TrackNum++;
1382                 m_Tracks.DelBlob(i-1);
1383             }/* end delete */
1384         }/* next track */
1385
1386         /* retraind data each 1 minute if new data exist */
1387         if(m_Frame%(25*60) == 0 && m_pTrainData && m_pTrainData->rows > m_LastTrainDataSize)
1388         {
1389             RetrainStatModel();
1390         }
1391
1392         m_Frame++;
1393
1394         if(m_Wnd && m_Dim==2)
1395         {/* debug output */
1396             int         x,y;
1397             IplImage*   pI = cvCloneImage(pImg);
1398
1399             if(m_pStatModel && m_pStatImg)
1400             for(y=0;y<pI->height;y+=2)
1401             {
1402                 uchar*  pStatData = (uchar*)m_pStatImg->imageData + y*m_pStatImg->widthStep;
1403                 uchar*  pData = (uchar*)pI->imageData + y*pI->widthStep;
1404                 for(x=0;x<pI->width;x+=2)
1405                 {/* draw all elements */
1406                     int d = pStatData[x];
1407                     d = (d<<8) | (d^0xff);
1408                     *(ushort*)(pData + x*3) = (ushort)d;
1409                 }
1410             }/* next line */
1411
1412             //cvNamedWindow("SVMMap",0);
1413             //cvShowImage("SVMMap", pI);
1414             cvReleaseImage(&pI);
1415         }/* debug output */
1416     };
1417     float GetState(int BlobID)
1418     {
1419         DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlobByID(BlobID);
1420         return pF?pF->state:0.0f;
1421     };
1422     /* return 0 if trajectory is normal
1423        return >0 if trajectory abnormal */
1424     virtual char*   GetStateDesc(int BlobID)
1425     {
1426         if(GetState(BlobID)>0.5) return "abnormal";
1427         return NULL;
1428     }
1429     virtual void    SetFileName(char* DataBaseName)
1430     {
1431         if(m_pTrainData)SaveStatModel();
1432         m_DataFileName[0] = 0;
1433         if(DataBaseName)
1434         {
1435             strncpy(m_DataFileName,DataBaseName,1000);
1436             strcat(m_DataFileName, ".yml");
1437         }
1438         LoadStatModel();
1439     };
1440
1441
1442     virtual void    Release(){ delete this; };
1443
1444 }; /* CvBlobTrackAnalysisSVM */
1445
1446
1447 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMP()
1448 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenP);}
1449 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMPV()
1450 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenPV);}
1451 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMPVS()
1452 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenPVS);}
1453 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMSS()
1454 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenSS);}