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