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