Update to 2.0.0 tree from current Fremantle build
[opencv] / src / cvaux / 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     const 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         SetModuleName("TrackDist");
313
314     } /* Constructor. */
315
316     ~CvBlobTrackAnalysisTrackDist()
317     {
318         int i;
319         for(i=m_Tracks.GetBlobNum(); i>0; --i)
320         {
321             DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
322             delete pF->pTrack;
323         }
324         if(m_pDebugImg) cvReleaseImage(&m_pDebugImg);
325         //if(m_pDebugAVI) cvReleaseVideoWriter(&m_pDebugAVI);
326     } /* Destructor. */
327
328     /*----------------- Interface: --------------------*/
329     virtual void    AddBlob(CvBlob* pBlob)
330     {
331         DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
332
333         if(pF == NULL)
334         {   /* Create new TRack record: */
335             DefTrackForDist F;
336             F.state = 0;
337             F.blob = pBlob[0];
338             F.LastFrame = m_Frame;
339             F.pTrack = new DefTrackRec(CV_BLOB_ID(pBlob));
340             m_Tracks.AddBlob((CvBlob*)&F);
341             pF = (DefTrackForDist*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
342         }
343
344         assert(pF);
345         assert(pF->pTrack);
346         pF->pTrack->AddPoint(pBlob->x,pBlob->y,pBlob->w*0.5f);
347         pF->blob = pBlob[0];
348         pF->LastFrame = m_Frame;
349     };
350
351     virtual void Process(IplImage* pImg, IplImage* /*pFG*/)
352     {
353         int i;
354         double          MinTv = pImg->width/1440.0; /* minimal threshold for speed difference */
355         double          MinTv2 = MinTv*MinTv;
356
357         for(i=m_Tracks.GetBlobNum(); i>0; --i)
358         {
359             DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
360             pF->state = 0;
361
362             if(pF->LastFrame == m_Frame || pF->LastFrame+1 == m_Frame)
363             {   /* Process one blob trajectory: */
364                 int NumEq = 0;
365                 int it;
366
367                 for(it=m_TrackDataBase.GetBlobNum(); it>0; --it)
368                 {   /* Check template: */
369                     DefTrackForDist*   pFT = (DefTrackForDist*)m_TrackDataBase.GetBlob(it-1);
370                     int         Num = pF->pTrack->GetPointNum();
371                     int         NumT = pFT->pTrack->GetPointNum();
372                     int*        pPairIdx = (int*)ReallocTempData(sizeof(int)*2*(Num+NumT)+sizeof(DefMatch)*Num*NumT);
373                     void*       pTmpData = pPairIdx+2*(Num+NumT);
374                     int         PairNum = 0;
375                     int         k;
376                     int         Equal = 1;
377                     int         UseVel = 0;
378                     int         UsePos = 0;
379
380                     if(i==it) continue;
381
382                     /* Match track: */
383                     PairNum = cvTrackMatch( pF->pTrack, m_TraceLen, pFT->pTrack, pPairIdx, pTmpData );
384                     Equal = MAX(1,cvRound(PairNum*0.1));
385
386                     UseVel = 3*pF->pTrack->GetPointNum() > m_TraceLen;
387                     UsePos = 10*pF->pTrack->GetPointNum() > m_TraceLen;
388
389                     {   /* Check continues: */
390                         float   D;
391                         int     DI = pPairIdx[0*2+0]-pPairIdx[(PairNum-1)*2+0];
392                         int     DIt = pPairIdx[0*2+1]-pPairIdx[(PairNum-1)*2+1];
393                         if(UseVel && DI != 0)
394                         {
395                             D = (float)(DI-DIt)/(float)DI;
396                             if(fabs(D)>m_VelThreshold)Equal=0;
397                             if(fabs(D)>m_VelThreshold*0.5)Equal/=2;
398                         }
399                     }   /* Check continues. */
400
401                     for(k=0; Equal>0 && k<PairNum; ++k)
402                     {   /* Compare with threshold: */
403                         int             j = pPairIdx[k*2+0];
404                         int             jt = pPairIdx[k*2+1];
405                         DefTrackPoint*  pB = pF->pTrack->GetPoint(j);
406                         DefTrackPoint*  pBT = pFT->pTrack->GetPoint(jt);
407                         double          dx = pB->x-pBT->x;
408                         double          dy = pB->y-pBT->y;
409                         double          dvx = pB->vx - pBT->vx;
410                         double          dvy = pB->vy - pBT->vy;
411                       //double          dv = pB->v - pBT->v;
412                         double          D = dx*dx+dy*dy;
413                         double          Td = pBT->r*m_PosThreshold;
414                         double          dv2 = dvx*dvx+dvy*dvy;
415                         double          Tv2 = (pBT->vx*pBT->vx+pBT->vy*pBT->vy)*m_VelThreshold*m_VelThreshold;
416                         double          Tvm = pBT->v*m_VelThreshold;
417
418
419                         if(Tv2 < MinTv2) Tv2 = MinTv2;
420                         if(Tvm < MinTv) Tvm = MinTv;
421
422                         /* Check trajectory position: */
423                         if(UsePos && D > Td*Td)
424                         {
425                             Equal--;
426                         }
427                         else
428                         /* Check trajectory velocity. */
429                         /* Don't consider trajectory tail because its unstable for velocity computation. */
430                         if(UseVel && j>5 && jt>5 && dv2 > Tv2 )
431                         {
432                             Equal--;
433                         }
434                     } /* Compare with threshold. */
435
436                     if(Equal>0)
437                     {
438                         NumEq++;
439                         pFT->close++;
440                     }
441                 } /* Next template. */
442
443                 {   /* Calculate state: */
444                     float   T = m_TrackDataBase.GetBlobNum() * m_AbnormalThreshold; /* calc threshold */
445
446                     if(T>0)
447                     {
448                         pF->state = (T - NumEq)/(T*0.2f) + 0.5f;
449                     }
450                     if(pF->state<0)pF->state=0;
451                     if(pF->state>1)pF->state=1;
452
453                     /*if(0)if(pF->state>0)
454                     {// if abnormal blob
455                         printf("Abnormal blob(%d) %d < %f, state=%f\n",CV_BLOB_ID(pF),NumEq,T, pF->state);
456                     }*/
457                 }   /* Calculate state. */
458             }   /*  Process one blob trajectory. */
459             else
460             {   /* Move track to tracks data base: */
461                 m_TrackDataBase.AddBlob((CvBlob*)pF);
462                 m_Tracks.DelBlob(i-1);
463             }
464         } /* Next blob. */
465
466
467         if(m_Wnd)
468         {   /* Debug output: */
469             int i;
470
471             if(m_pDebugImg==NULL)
472                 m_pDebugImg = cvCloneImage(pImg);
473             else
474                 cvCopyImage(pImg, m_pDebugImg);
475
476             for(i=m_TrackDataBase.GetBlobNum(); i>0; --i)
477             {   /* Draw all elements in track data base:  */
478                 int         j;
479                 DefTrackForDist*   pF = (DefTrackForDist*)m_TrackDataBase.GetBlob(i-1);
480                 CvScalar    color = CV_RGB(0,0,0);
481                 if(!pF->close) continue;
482                 if(pF->close)
483                 {
484                     color = CV_RGB(0,0,255);
485                 }
486                 else
487                 {
488                     color = CV_RGB(0,0,128);
489                 }
490
491                 for(j=pF->pTrack->GetPointNum(); j>0; j--)
492                 {
493                     DefTrackPoint* pB = pF->pTrack->GetPoint(j-1);
494                     int r = 0;//MAX(cvRound(pB->r),1);
495                     cvCircle(m_pDebugImg, cvPoint(cvRound(pB->x),cvRound(pB->y)), r, color);
496                 }
497                 pF->close = 0;
498             }   /* Draw all elements in track data base. */
499
500             for(i=m_Tracks.GetBlobNum(); i>0; --i)
501             {   /* Draw all elements for all trajectories: */
502                 DefTrackForDist*    pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
503                 int                 j;
504                 int                 c = cvRound(pF->state*255);
505                 CvScalar            color = CV_RGB(c,255-c,0);
506                 CvPoint             p = cvPointFrom32f(CV_BLOB_CENTER(pF));
507                 int                 x = cvRound(CV_BLOB_RX(pF)), y = cvRound(CV_BLOB_RY(pF));
508                 CvSize              s = cvSize(MAX(1,x), MAX(1,y));
509
510                 cvEllipse( m_pDebugImg,
511                     p,
512                     s,
513                     0, 0, 360,
514                     CV_RGB(c,255-c,0), cvRound(1+(0*c)/255) );
515
516                 for(j=pF->pTrack->GetPointNum(); j>0; j--)
517                 {
518                     DefTrackPoint* pB = pF->pTrack->GetPoint(j-1);
519                     if(pF->pTrack->GetPointNum()-j > m_TraceLen) break;
520                     cvCircle(m_pDebugImg, cvPoint(cvRound(pB->x),cvRound(pB->y)), 0, color);
521                 }
522                 pF->close = 0;
523
524             }   /* Draw all elements for all trajectories. */
525
526             //cvNamedWindow("Tracks",0);
527             //cvShowImage("Tracks", m_pDebugImg);
528         } /* Debug output. */
529
530 #if 0
531         if(m_pDebugImg && m_pDebugAVIName)
532         {
533             if(m_pDebugAVI==NULL)
534             {   /* Create avi file for writing: */
535                 m_pDebugAVI = cvCreateVideoWriter(
536                     m_pDebugAVIName,
537                     CV_FOURCC('x','v','i','d'),
538                     25,
539                     cvSize(m_pDebugImg->width,m_pDebugImg->height));
540
541                 if(m_pDebugAVI == NULL)
542                 {
543                     printf("WARNING!!! Can not create AVI file %s for writing\n",m_pDebugAVIName);
544                 }
545             }   /* Create avi file for writing. */
546
547             if(m_pDebugAVI)cvWriteFrame( m_pDebugAVI, m_pDebugImg );
548         }   /* Write debug window to AVI file. */
549 #endif
550         m_Frame++;
551     };
552     float GetState(int BlobID)
553     {
554         DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlobByID(BlobID);
555         return pF?pF->state:0.0f;
556     };
557
558     /* Return 0 if trajectory is normal;
559        return >0 if trajectory abnormal. */
560     virtual const char*   GetStateDesc(int BlobID)
561     {
562         if(GetState(BlobID)>0.5) return "abnormal";
563         return NULL;
564     }
565
566     virtual void    SetFileName(char* DataBaseName)
567     {
568         m_DataFileName[0] = 0;
569         if(DataBaseName)
570         {
571             strncpy(m_DataFileName,DataBaseName,1000);
572             strcat(m_DataFileName, ".yml");
573         }
574     };
575
576     virtual void    Release(){ delete this; };
577 };
578
579
580
581 CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisTrackDist()
582 {return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisTrackDist;}
583