git.maemo.org Git - opencv/blob - cvaux/src/vs/blobtrackanalysistrackdist.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 typedef struct DefTrackPoint
  45 {
  46     float x,y,r,vx,vy,v;
  47 } DefTrackPoint;
  48
  49 class DefTrackRec
  50 {
  51 private:
  52     int     ID;
  53 public:
  54     DefTrackRec(int id = 0,int BlobSize = sizeof(DefTrackPoint))
  55     {
  56         ID = id;
  57         m_pMem = cvCreateMemStorage();
  58         m_pSeq = cvCreateSeq(0,sizeof(CvSeq),BlobSize,m_pMem);
  59     }
  60     ~DefTrackRec()
  61     {
  62         cvReleaseMemStorage(&m_pMem);
  63     };
  64     inline DefTrackPoint* GetPoint(int PointIndex)
  65     {
  66         return (DefTrackPoint*)cvGetSeqElem(m_pSeq,PointIndex);
  67     };
  68     inline void DelPoint(int PointIndex)
  69     {
  70         cvSeqRemove(m_pSeq,PointIndex);
  71     };
  72     inline void Clear()
  73     {
  74         cvClearSeq(m_pSeq);
  75     };
  76     inline void AddPoint(float x, float y, float r)
  77     {
  78         DefTrackPoint   p = {x,y,r,0};
  79         int             Num = GetPointNum();
  80
  81         if(Num > 0)
  82         {
  83             DefTrackPoint* pPrev = GetPoint(Num-1);
  84             float   Alpha = 0.8f;
  85             float  dx = x-pPrev->x;
  86             float  dy = y-pPrev->y;
  87             p.vx = Alpha*dx+(1-Alpha)*pPrev->vx;
  88             p.vy = Alpha*dy+(1-Alpha)*pPrev->vy;
  89             p.v  = Alpha*dx+(1-Alpha)*pPrev->v;
  90         }
  91         AddPoint(&p);
  92     }
  93
  94     inline void AddPoint(DefTrackPoint* pB)
  95     {   /* Add point and recalculate last velocities: */
  96         int     wnd=3;
  97         int     Num;
  98         int     i;
  99         cvSeqPush(m_pSeq,pB);
 100
 101         Num = GetPointNum();
 102
 103         for(i=MAX(0,Num-wnd-1); i<Num; ++i)
 104         {   /* Next updating point: */
 105             DefTrackPoint*  p = GetPoint(i);
 106             int             j0 = i - wnd;
 107             int             j1 = i + wnd;
 108
 109             if(j0<0) j0 = 0;
 110             if(j1>=Num)j1=Num-1;
 111
 112             if(j1>j0)
 113             {
 114                 float           dt = (float)(j1-j0);
 115                 DefTrackPoint*  p0 = GetPoint(j0);
 116                 DefTrackPoint*  p1 = GetPoint(j1);
 117                 p->vx = (p1->x - p0->x) / dt;
 118                 p->vy = (p1->y - p0->y) / dt;
 119                 p->v = (float)sqrt(p->vx*p->vx+p->vy*p->vy);
 120             }
 121         } /* Next updating point. */
 122
 123 #if 0
 124         if(0)
 125         {   /* Debug: */
 126             int i;
 127             printf("Blob %d: ",ID);
 128
 129             for(i=0; i<GetPointNum(); ++i)
 130             {
 131                 DefTrackPoint*  p = GetPoint(i);
 132                 printf(",(%.2f,%.2f,%f.2)",p->vx,p->vy,p->v);
 133             }
 134             printf("\n");
 135         }
 136 #endif
 137     };
 138     inline int GetPointNum()
 139     {
 140         return m_pSeq->total;
 141     };
 142 private:
 143     CvMemStorage*   m_pMem;
 144     CvSeq*          m_pSeq;
 145 };
 146
 147 /* Fill array pIdxPairs by pair of index of correspondent blobs. */
 148 /* Return number of pairs.                                       */
 149 /* pIdxPairs must have size not less that 2*(pSeqNum+pSeqTNum)   */
 150 /* pTmp is pointer to memory which size is pSeqNum*pSeqTNum*16   */
 151 typedef struct DefMatch
 152 {
 153     int     Idx;  /* Previous best blob index.          */
 154     int     IdxT; /* Previous best template blob index. */
 155     double  D;    /* Blob to blob distance sum.         */
 156 } DefMatch;
 157
 158 static int cvTrackMatch(DefTrackRec* pSeq, int MaxLen, DefTrackRec* pSeqT, int* pIdxPairs, void* pTmp)
 159 {
 160     int         NumPair = 0;
 161     DefMatch*   pMT = (DefMatch*)pTmp;
 162     int         Num = pSeq->GetPointNum();
 163     int         NumT = pSeqT->GetPointNum();
 164     int         i,it;
 165     int         i0=0; /* Last point in the track sequence. */
 166
 167     if(MaxLen > 0 && Num > MaxLen)
 168     {   /* Set new point seq len and new last point in this seq: */
 169         Num = MaxLen;
 170         i0 = pSeq->GetPointNum() - Num;
 171     }
 172
 173     for(i=0; i<Num; ++i)
 174     {   /* For each point row: */
 175         for(it=0; it<NumT; ++it)
 176         {   /* For each point template column: */
 177             DefTrackPoint*  pB = pSeq->GetPoint(i+i0);
 178             DefTrackPoint*  pBT = pSeqT->GetPoint(it);
 179             DefMatch*       pMT_cur = pMT + i*NumT + it;
 180             double          dx = pB->x-pBT->x;
 181             double          dy = pB->y-pBT->y;
 182             double          D = dx*dx+dy*dy;
 183             int             DI[3][2] = {{-1,-1},{-1,0},{0,-1}};
 184             int             iDI;
 185
 186             pMT_cur->D = D;
 187             pMT_cur->Idx = -1;
 188             pMT_cur->IdxT = 0;
 189
 190             if(i==0) continue;
 191
 192             for(iDI=0; iDI<3; ++iDI)
 193             {
 194                 int         i_prev = i+DI[iDI][0];
 195                 int         it_prev = it+DI[iDI][1];
 196
 197                 if(i_prev >= 0 && it_prev>=0)
 198                 {
 199                     double D_cur = D+pMT[NumT*i_prev+it_prev].D;
 200
 201                     if(pMT_cur->D > D_cur || (pMT_cur->Idx<0) )
 202                     {   /* Set new best local way: */
 203                         pMT_cur->D = D_cur;
 204                         pMT_cur->Idx = i_prev;
 205                         pMT_cur->IdxT = it_prev;
 206                     }
 207                 }
 208             } /* Check next direction. */
 209         } /* Fill next colum from table. */
 210     } /* Fill next row. */
 211
 212     {   /* Back tracking. */
 213         /* Find best end in template: */
 214         int         it_best = 0;
 215         DefMatch*   pMT_best = pMT + (Num-1)*NumT;
 216         i = Num-1; /* set current i to last position */
 217
 218         for(it=1; it<NumT; ++it)
 219         {
 220             DefMatch* pMT_new = pMT + it + i*NumT;
 221
 222             if(pMT_best->D > pMT_new->D)
 223             {
 224                 pMT_best->D = pMT_new->D;
 225                 it_best = it;
 226             }
 227         } /* Find best end template point. */
 228
 229         /* Back tracking whole sequence: */
 230         for(it = it_best;i>=0 && it>=0;)
 231         {
 232             DefMatch* pMT_new = pMT + it + i*NumT;
 233             pIdxPairs[2*NumPair] = i+i0;
 234             pIdxPairs[2*NumPair+1] = it;
 235             NumPair++;
 236
 237             it = pMT_new->IdxT;
 238             i = pMT_new->Idx;
 239         }
 240     } /* End back tracing. */
 241
 242     return NumPair;
 243 } /* cvTrackMatch. */
 244
 245 typedef struct DefTrackForDist
 246 {
 247     CvBlob                  blob;
 248     DefTrackRec*            pTrack;
 249     int                     LastFrame;
 250     float                   state;
 251     /* for debug */
 252     int                     close;
 253 } DefTrackForDist;
 254
 255 class CvBlobTrackAnalysisTrackDist : public CvBlobTrackAnalysis
 256 {
 257     /*---------------- Internal functions: --------------------*/
 258 private:
 259     char*               m_pDebugAVIName; /* For debugging. */
 260   //CvVideoWriter*      m_pDebugAVI;     /* For debugging. */
 261     IplImage*           m_pDebugImg;     /* For debugging. */
 262
 263     char                m_DataFileName[1024];
 264     CvBlobSeq           m_Tracks;
 265     CvBlobSeq           m_TrackDataBase;
 266     int                 m_Frame;
 267     void*               m_pTempData;
 268     int                 m_TempDataSize;
 269     int                 m_TraceLen;
 270     float               m_AbnormalThreshold;
 271     float               m_PosThreshold;
 272     float               m_VelThreshold;
 273     inline void* ReallocTempData(int Size)
 274     {
 275         if(Size <= m_TempDataSize && m_pTempData) return m_pTempData;
 276         cvFree(&m_pTempData);
 277         m_TempDataSize = 0;
 278         m_pTempData = cvAlloc(Size);
 279         if(m_pTempData) m_TempDataSize = Size;
 280         return m_pTempData;
 281     } /* ReallocTempData. */
 282
 283 public:
 284     CvBlobTrackAnalysisTrackDist():m_Tracks(sizeof(DefTrackForDist)),m_TrackDataBase(sizeof(DefTrackForDist))
 285     {
 286         m_pDebugImg = 0;
 287         //m_pDebugAVI = 0;
 288         m_Frame = 0;
 289         m_pTempData = NULL;
 290         m_TempDataSize = 0;
 291
 292         m_pDebugAVIName = NULL;
 293         AddParam("DebugAVI",&m_pDebugAVIName);
 294         CommentParam("DebugAVI","Name of AVI file to save images from debug window");
 295
 296         m_TraceLen = 50;
 297         AddParam("TraceLen",&m_TraceLen);
 298         CommentParam("TraceLen","Length (in frames) of trajectory part that is used for comparison");
 299
 300         m_AbnormalThreshold = 0.02f;
 301         AddParam("AbnormalThreshold",&m_AbnormalThreshold);
 302         CommentParam("AbnormalThreshold","If trajectory is equal with less then <AbnormalThreshold*DataBaseTrackNum> tracks then trajectory is abnormal");
 303
 304         m_PosThreshold = 1.25;
 305         AddParam("PosThreshold",&m_PosThreshold);
 306         CommentParam("PosThreshold","Minimal allowd distance in blob width that is allowed");
 307
 308         m_VelThreshold = 0.5;
 309         AddParam("VelThreshold",&m_VelThreshold);
 310         CommentParam("VelThreshold","Minimal allowed relative difference between blob speed");
 311
 312     } /* Constructor. */
 313
 314     ~CvBlobTrackAnalysisTrackDist()
 315     {
 316         int i;
 317         for(i=m_Tracks.GetBlobNum(); i>0; --i)
 318         {
 319             DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
 320             delete pF->pTrack;
 321         }
 322         if(m_pDebugImg) cvReleaseImage(&m_pDebugImg);
 323         //if(m_pDebugAVI) cvReleaseVideoWriter(&m_pDebugAVI);
 324     } /* Destructor. */
 325
 326     /*----------------- Interface: --------------------*/
 327     virtual void    AddBlob(CvBlob* pBlob)
 328     {
 329         DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
 330
 331         if(pF == NULL)
 332         {   /* Create new TRack record: */
 333             DefTrackForDist F;
 334             F.state = 0;
 335             F.blob = pBlob[0];
 336             F.LastFrame = m_Frame;
 337             F.pTrack = new DefTrackRec(CV_BLOB_ID(pBlob));
 338             m_Tracks.AddBlob((CvBlob*)&F);
 339             pF = (DefTrackForDist*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
 340         }
 341
 342         assert(pF);
 343         assert(pF->pTrack);
 344         pF->pTrack->AddPoint(pBlob->x,pBlob->y,pBlob->w*0.5f);
 345         pF->blob = pBlob[0];
 346         pF->LastFrame = m_Frame;
 347     };
 348
 349     virtual void Process(IplImage* pImg, IplImage* /*pFG*/)
 350     {
 351         int i;
 352         double          MinTv = pImg->width/1440.0; /* minimal threshold for speed difference */
 353         double          MinTv2 = MinTv*MinTv;
 354
 355         for(i=m_Tracks.GetBlobNum(); i>0; --i)
 356         {
 357             DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
 358             pF->state = 0;
 359
 360             if(pF->LastFrame == m_Frame || pF->LastFrame+1 == m_Frame)
 361             {   /* Process one blob trajectory: */
 362                 int NumEq = 0;
 363                 int it;
 364
 365                 for(it=m_TrackDataBase.GetBlobNum(); it>0; --it)
 366                 {   /* Check template: */
 367                     DefTrackForDist*   pFT = (DefTrackForDist*)m_TrackDataBase.GetBlob(it-1);
 368                     int         Num = pF->pTrack->GetPointNum();
 369                     int         NumT = pFT->pTrack->GetPointNum();
 370                     int*        pPairIdx = (int*)ReallocTempData(sizeof(int)*2*(Num+NumT)+sizeof(DefMatch)*Num*NumT);
 371                     void*       pTmpData = pPairIdx+2*(Num+NumT);
 372                     int         PairNum = 0;
 373                     int         k;
 374                     int         Equal = 1;
 375                     int         UseVel = 0;
 376                     int         UsePos = 0;
 377
 378                     if(i==it) continue;
 379
 380                     /* Match track: */
 381                     PairNum = cvTrackMatch( pF->pTrack, m_TraceLen, pFT->pTrack, pPairIdx, pTmpData );
 382                     Equal = MAX(1,cvRound(PairNum*0.1));
 383
 384                     UseVel = 3*pF->pTrack->GetPointNum() > m_TraceLen;
 385                     UsePos = 10*pF->pTrack->GetPointNum() > m_TraceLen;
 386
 387                     {   /* Check continues: */
 388                         float   D;
 389                         int     DI = pPairIdx[0*2+0]-pPairIdx[(PairNum-1)*2+0];
 390                         int     DIt = pPairIdx[0*2+1]-pPairIdx[(PairNum-1)*2+1];
 391                         if(UseVel && DI != 0)
 392                         {
 393                             D = (float)(DI-DIt)/(float)DI;
 394                             if(fabs(D)>m_VelThreshold)Equal=0;
 395                             if(fabs(D)>m_VelThreshold*0.5)Equal/=2;
 396                         }
 397                     }   /* Check continues. */
 398
 399                     for(k=0; Equal>0 && k<PairNum; ++k)
 400                     {   /* Compare with threshold: */
 401                         int             j = pPairIdx[k*2+0];
 402                         int             jt = pPairIdx[k*2+1];
 403                         DefTrackPoint*  pB = pF->pTrack->GetPoint(j);
 404                         DefTrackPoint*  pBT = pFT->pTrack->GetPoint(jt);
 405                         double          dx = pB->x-pBT->x;
 406                         double          dy = pB->y-pBT->y;
 407                         double          dvx = pB->vx - pBT->vx;
 408                         double          dvy = pB->vy - pBT->vy;
 409                       //double          dv = pB->v - pBT->v;
 410                         double          D = dx*dx+dy*dy;
 411                         double          Td = pBT->r*m_PosThreshold;
 412                         double          dv2 = dvx*dvx+dvy*dvy;
 413                         double          Tv2 = (pBT->vx*pBT->vx+pBT->vy*pBT->vy)*m_VelThreshold*m_VelThreshold;
 414                         double          Tvm = pBT->v*m_VelThreshold;
 415
 416
 417                         if(Tv2 < MinTv2) Tv2 = MinTv2;
 418                         if(Tvm < MinTv) Tvm = MinTv;
 419
 420                         /* Check trajectory position: */
 421                         if(UsePos && D > Td*Td)
 422                         {
 423                             Equal--;
 424                         }
 425                         else
 426                         /* Check trajectory velocity. */
 427                         /* Don't consider trajectory tail because its unstable for velocity computation. */
 428                         if(UseVel && j>5 && jt>5 && dv2 > Tv2 )
 429                         {
 430                             Equal--;
 431                         }
 432                     } /* Compare with threshold. */
 433
 434                     if(Equal>0)
 435                     {
 436                         NumEq++;
 437                         pFT->close++;
 438                     }
 439                 } /* Next template. */
 440
 441                 {   /* Calculate state: */
 442                     float   T = m_TrackDataBase.GetBlobNum() * m_AbnormalThreshold; /* calc threshold */
 443
 444                     if(T>0)
 445                     {
 446                         pF->state = (T - NumEq)/(T*0.2f) + 0.5f;
 447                     }
 448                     if(pF->state<0)pF->state=0;
 449                     if(pF->state>1)pF->state=1;
 450
 451                     /*if(0)if(pF->state>0)
 452                     {// if abnormal blob
 453                         printf("Abnormal blob(%d) %d < %f, state=%f\n",CV_BLOB_ID(pF),NumEq,T, pF->state);
 454                     }*/
 455                 }   /* Calculate state. */
 456             }   /*  Process one blob trajectory. */
 457             else
 458             {   /* Move track to tracks data base: */
 459                 m_TrackDataBase.AddBlob((CvBlob*)pF);
 460                 m_Tracks.DelBlob(i-1);
 461             }
 462         } /* Next blob. */
 463
 464
 465         if(m_Wnd)
 466         {   /* Debug output: */
 467             int i;
 468
 469             if(m_pDebugImg==NULL)
 470                 m_pDebugImg = cvCloneImage(pImg);
 471             else
 472                 cvCopyImage(pImg, m_pDebugImg);
 473
 474             for(i=m_TrackDataBase.GetBlobNum(); i>0; --i)
 475             {   /* Draw all elements in track data base:  */
 476                 int         j;
 477                 DefTrackForDist*   pF = (DefTrackForDist*)m_TrackDataBase.GetBlob(i-1);
 478                 CvScalar    color = CV_RGB(0,0,0);
 479                 if(!pF->close) continue;
 480                 if(pF->close)
 481                 {
 482                     color = CV_RGB(0,0,255);
 483                 }
 484                 else
 485                 {
 486                     color = CV_RGB(0,0,128);
 487                 }
 488
 489                 for(j=pF->pTrack->GetPointNum(); j>0; j--)
 490                 {
 491                     DefTrackPoint* pB = pF->pTrack->GetPoint(j-1);
 492                     int r = 0;//MAX(cvRound(pB->r),1);
 493                     cvCircle(m_pDebugImg, cvPoint(cvRound(pB->x),cvRound(pB->y)), r, color);
 494                 }
 495                 pF->close = 0;
 496             }   /* Draw all elements in track data base. */
 497
 498             for(i=m_Tracks.GetBlobNum(); i>0; --i)
 499             {   /* Draw all elements for all trajectories: */
 500                 DefTrackForDist*    pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
 501                 int                 j;
 502                 int                 c = cvRound(pF->state*255);
 503                 CvScalar            color = CV_RGB(c,255-c,0);
 504                 CvPoint             p = cvPointFrom32f(CV_BLOB_CENTER(pF));
 505                 int                 x = cvRound(CV_BLOB_RX(pF)), y = cvRound(CV_BLOB_RY(pF));
 506                 CvSize              s = cvSize(MAX(1,x), MAX(1,y));
 507
 508                 cvEllipse( m_pDebugImg,
 509                     p,
 510                     s,
 511                     0, 0, 360,
 512                     CV_RGB(c,255-c,0), cvRound(1+(0*c)/255) );
 513
 514                 for(j=pF->pTrack->GetPointNum(); j>0; j--)
 515                 {
 516                     DefTrackPoint* pB = pF->pTrack->GetPoint(j-1);
 517                     if(pF->pTrack->GetPointNum()-j > m_TraceLen) break;
 518                     cvCircle(m_pDebugImg, cvPoint(cvRound(pB->x),cvRound(pB->y)), 0, color);
 519                 }
 520                 pF->close = 0;
 521
 522             }   /* Draw all elements for all trajectories. */
 523
 524             //cvNamedWindow("Tracks",0);
 525             //cvShowImage("Tracks", m_pDebugImg);
 526         } /* Debug output. */
 527
 528 #if 0
 529         if(m_pDebugImg && m_pDebugAVIName)
 530         {
 531             if(m_pDebugAVI==NULL)
 532             {   /* Create avi file for writing: */
 533                 m_pDebugAVI = cvCreateVideoWriter(
 534                     m_pDebugAVIName,
 535                     CV_FOURCC('x','v','i','d'),
 536                     25,
 537                     cvSize(m_pDebugImg->width,m_pDebugImg->height));
 538
 539                 if(m_pDebugAVI == NULL)
 540                 {
 541                     printf("WARNING!!! Can not create AVI file %s for writing\n",m_pDebugAVIName);
 542                 }
 543             }   /* Create avi file for writing. */
 544
 545             if(m_pDebugAVI)cvWriteFrame( m_pDebugAVI, m_pDebugImg );
 546         }   /* Write debug window to AVI file. */
 547 #endif
 548         m_Frame++;
 549     };
 550     float GetState(int BlobID)
 551     {
 552         DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlobByID(BlobID);
 553         return pF?pF->state:0.0f;
 554     };
 555
 556     /* Return 0 if trajectory is normal;
 557        return >0 if trajectory abnormal. */
 558     virtual char*   GetStateDesc(int BlobID)
 559     {
 560         if(GetState(BlobID)>0.5) return "abnormal";
 561         return NULL;
 562     }
 563
 564     virtual void    SetFileName(char* DataBaseName)
 565     {
 566         m_DataFileName[0] = 0;
 567         if(DataBaseName)
 568         {
 569             strncpy(m_DataFileName,DataBaseName,1000);
 570             strcat(m_DataFileName, ".yml");
 571         }
 572     };
 573
 574     virtual void    Release(){ delete this; };
 575 };
 576
 577
 578
 579 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisTrackDist()
 580 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisTrackDist;}
 581