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