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