Update the changelog
[opencv] / cvaux / src / vs / blobtrackinglist.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 //
12 // Copyright (C) 2000, Intel Corporation, all rights reserved.
13 // Third party copyrights are property of their respective owners.
14 //
15 // Redistribution and use in source and binary forms, with or without modification,
16 // are permitted provided that the following conditions are met:
17 //
18 //   * Redistribution's of source code must retain the above copyright notice,
19 //     this list of conditions and the following disclaimer.
20 //
21 //   * Redistribution's in binary form must reproduce the above copyright notice,
22 //     this list of conditions and the following disclaimer in the documentation
23 //     and/or other materials provided with the distribution.
24 //
25 //   * The name of Intel Corporation may not be used to endorse or promote products
26 //     derived from this software without specific prior written permission.
27 //
28 // This software is provided by the copyright holders and contributors "as is" and
29 // any express or implied warranties, including, but not limited to, the implied
30 // warranties of merchantability and fitness for a particular purpose are disclaimed.
31 // In no event shall the Intel Corporation or contributors be liable for any direct,
32 // indirect, incidental, special, exemplary, or consequential damages
33 // (including, but not limited to, procurement of substitute goods or services;
34 // loss of use, data, or profits; or business interruption) however caused
35 // and on any theory of liability, whether in contract, strict liability,
36 // or tort (including negligence or otherwise) arising in any way out of
37 // the use of this software, even if advised of the possibility of such damage.
38 //
39 //M*/
40
41 #include "_cvaux.h"
42
43 #define  PIX_HIST_BIN_NUM_1  3 //number of bins for classification (not used now)
44 #define  PIX_HIST_BIN_NUM_2  5 //number of bins for statistic collection
45 #define  PIX_HIST_ALPHA      0.01f //alpha-coefficient for running avarage procedure
46 #define  PIX_HIST_DELTA      2 //maximal difference between descriptors(RGB)
47 #define  PIX_HIST_COL_QUANTS 64 //quantization level in rgb-space
48 #define  PIX_HIST_DELTA_IN_PIX_VAL  (PIX_HIST_DELTA * 256 / PIX_HIST_COL_QUANTS) //allowed difference in rgb-space
49
50 // Structures for background statistics estimation:
51 typedef struct CvPixHistBin{
52     float          bin_val;
53     uchar          cols[3];
54 } CvPixHistBin;
55
56 typedef struct CvPixHist{
57     CvPixHistBin   bins[PIX_HIST_BIN_NUM_2];
58 } CvPixHist;
59
60 // Class for background statistics estimation:
61 class CvBGEstimPixHist
62 {
63 private:
64     CvPixHist*  m_PixHists;
65     int         m_width;
66     int         m_height;
67
68     // Function for update color histogram for one pixel:
69     void update_hist_elem(int x, int y, uchar* cols )
70     {
71         // Find closest bin:
72         int    dist = 0, min_dist = 2147483647, indx = -1;
73         for( int k = 0; k < PIX_HIST_BIN_NUM_2; k++ ){
74
75             uchar* hist_cols = m_PixHists[y*m_width+x].bins[k].cols;
76
77             m_PixHists[y*m_width+x].bins[k].bin_val *= (1-PIX_HIST_ALPHA);
78
79             int  l;
80             for( l = 0; l < 3; l++ ){
81                 int val = abs( hist_cols[l] - cols[l] );
82                 if( val > PIX_HIST_DELTA_IN_PIX_VAL ) break;
83                 dist += val;
84             }
85
86             if( l == 3 && dist < min_dist ){
87                 min_dist = dist;
88                 indx = k;
89             }
90         }
91         if( indx < 0 ){   // N2th elem in the table is replaced by a new feature.
92             indx = PIX_HIST_BIN_NUM_2 - 1;
93             m_PixHists[y*m_width+x].bins[indx].bin_val = PIX_HIST_ALPHA;
94             for(int l = 0; l < 3; l++ ){
95                 m_PixHists[y*m_width+x].bins[indx].cols[l] = cols[l];
96             }
97         }
98         else {
99             //add vote!
100             m_PixHists[y*m_width+x].bins[indx].bin_val += PIX_HIST_ALPHA;
101         }
102         // Re-sort bins by BIN_VAL:
103         {
104             int k;
105             for(k = 0; k < indx; k++ ){
106                 if( m_PixHists[y*m_width+x].bins[k].bin_val <= m_PixHists[y*m_width+x].bins[indx].bin_val ){
107                     CvPixHistBin tmp1, tmp2 = m_PixHists[y*m_width+x].bins[indx];
108                     // Shift elements:
109                     for(int l = k; l <= indx; l++ ){
110                         tmp1 = m_PixHists[y*m_width+x].bins[l];
111                         m_PixHists[y*m_width+x].bins[l] = tmp2;
112                         tmp2 = tmp1;
113                     }
114                     break;
115                 }
116             }
117         }
118     }   // void update_hist(...)
119
120     // Function for calculation difference between histograms:
121     float get_hist_diff(int x1, int y1, int x2, int y2)
122     {
123         float  dist = 0;
124         for( int i = 0; i < 3; i++ ){
125             dist += labs(m_PixHists[y1*m_width+x1].bins[0].cols[i] -
126                                 m_PixHists[y2*m_width+x2].bins[0].cols[i]);
127         }
128         return dist;
129     }
130
131
132 public:
133     IplImage*   bg_image;
134
135     CvBGEstimPixHist(CvSize img_size)
136     {
137         m_PixHists = (CvPixHist*)cvAlloc(img_size.width*img_size.height*sizeof(CvPixHist));
138         memset( m_PixHists, 0, img_size.width*img_size.height*sizeof(CvPixHist) );
139         m_width = img_size.width;
140         m_height = img_size.height;
141
142         bg_image = cvCreateImage(img_size, IPL_DEPTH_8U, 3 );
143     }   /* Constructor. */
144
145     ~CvBGEstimPixHist()
146     {
147         cvReleaseImage(&bg_image);
148         cvFree(&m_PixHists);
149     }   /* Destructor. */
150
151     // Function to update histograms and bg_image:
152     void update_hists( IplImage* pImg )
153     {
154         for( int i = 0; i < pImg->height; i++ ){
155             for( int j = 0; j < pImg->width; j++ ){
156                 update_hist_elem( j, i, ((uchar*)(pImg->imageData))+i*pImg->widthStep+3*j );
157                 ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j] = m_PixHists[i*m_width+j].bins[0].cols[0];
158                 ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j+1] = m_PixHists[i*m_width+j].bins[0].cols[1];
159                 ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j+2] = m_PixHists[i*m_width+j].bins[0].cols[2];
160             }
161         }
162         // cvNamedWindow("RoadMap2",0);
163         // cvShowImage("RoadMap2", bg_image);
164     }
165 };  /* CvBGEstimPixHist */
166
167
168
169 /*======================= TRACKER LIST SHELL =====================*/
170 typedef struct DefBlobTrackerL
171 {
172     CvBlob                  blob;
173     CvBlobTrackerOne*       pTracker;
174     int                     Frame;
175     int                     Collision;
176     CvBlobTrackPredictor*   pPredictor;
177     CvBlob                  BlobPredict;
178     CvBlobSeq*              pBlobHyp;
179 } DefBlobTrackerL;
180
181 class CvBlobTrackerList : public CvBlobTracker
182 {
183 private:
184     CvBlobTrackerOne*       (*m_Create)();
185     CvBlobSeq               m_BlobTrackerList;
186 //    int                     m_LastID;
187     int                     m_Collision;
188     int                     m_ClearHyp;
189     float                   m_BGImageUsing;
190     CvBGEstimPixHist*       m_pBGImage;
191     IplImage*               m_pImgFG;
192     IplImage*               m_pImgReg; /* mask for multiblob confidence calculation */
193
194 public:
195     CvBlobTrackerList(CvBlobTrackerOne* (*create)()):m_BlobTrackerList(sizeof(DefBlobTrackerL))
196     {
197         //int i;
198         CvBlobTrackerOne* pM = create();
199 //        m_LastID = 0;
200         m_Create = create;
201         m_ClearHyp = 0;
202         m_pImgFG = 0;
203         m_pImgReg = NULL;
204
205         TransferParamsFromChild(pM,NULL);
206
207         pM->Release();
208
209         m_Collision = 1; /* if 1 then collistion will be detected and processed */
210         AddParam("Collision",&m_Collision);
211         CommentParam("Collision", "if 1 then collision cases are processed in special way");
212
213         m_pBGImage = NULL;
214         m_BGImageUsing = 50;
215         AddParam("BGImageUsing", &m_BGImageUsing);
216         CommentParam("BGImageUsing","Weight of using BG image in update hist model (0 - BG dies not use 1 - use)");
217     }
218
219     ~CvBlobTrackerList()
220     {
221         int i;
222         if(m_pBGImage) delete m_pBGImage;
223         if(m_pImgFG) cvReleaseImage(&m_pImgFG);
224         if(m_pImgReg) cvReleaseImage(&m_pImgReg);
225         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
226         {
227             m_BlobTrackerList.DelBlob(i-1);
228         }
229     };
230
231     CvBlob* AddBlob(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG )
232     {   /* Create new tracker: */
233         DefBlobTrackerL F;
234         F.blob = pBlob[0];
235 //        F.blob.ID = m_LastID++;
236         F.pTracker = m_Create();
237         F.pPredictor = cvCreateModuleBlobTrackPredictKalman();
238         F.pBlobHyp = new CvBlobSeq;
239         F.Frame = 0;
240         TransferParamsToChild(F.pTracker,NULL);
241
242         F.pTracker->Init(pBlob,pImg, pImgFG);
243         m_BlobTrackerList.AddBlob((CvBlob*)&F);
244         return m_BlobTrackerList.GetBlob(m_BlobTrackerList.GetBlobNum()-1);
245     };
246
247     void DelBlob(int BlobIndex)
248     {
249         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
250         if(pF == NULL) return;
251         pF->pTracker->Release();
252         pF->pPredictor->Release();
253         delete pF->pBlobHyp;
254         m_BlobTrackerList.DelBlob(BlobIndex);
255     }
256
257     void DelBlobByID(int BlobID)
258     {
259         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(BlobID);
260         if(pF == NULL) return;
261         pF->pTracker->Release();
262         pF->pPredictor->Release();
263         delete pF->pBlobHyp;
264         m_BlobTrackerList.DelBlobByID(BlobID);
265     }
266
267     virtual void Process(IplImage* pImg, IplImage* pImgFG = NULL)
268     {
269         int i;
270         if(pImgFG)
271         {
272             if(m_pImgFG) cvCopyImage(pImgFG,m_pImgFG);
273             else         m_pImgFG = cvCloneImage(pImgFG);
274         }
275
276         if(m_pBGImage==NULL && m_BGImageUsing>0)
277         {
278             m_pBGImage = new CvBGEstimPixHist(cvSize(pImg->width,pImg->height));
279         }
280
281         if(m_Collision)
282         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
283         {   /* Update predictor: */
284             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
285             pF->pPredictor->Update((CvBlob*)pF);
286         }   /* Update predictor. */
287
288         if(m_pBGImage && m_pImgFG)
289         {   /* Weighting mask mask: */
290             int x,y,yN=pImg->height,xN=pImg->width;
291             IplImage* pImgBG = NULL;
292             m_pBGImage->update_hists(pImg);
293             pImgBG = m_pBGImage->bg_image;
294
295             for(y=0; y<yN; ++y)
296             {
297                 unsigned char* pI = (unsigned char*)pImg->imageData + y*pImg->widthStep;
298                 unsigned char* pBG = (unsigned char*)pImgBG->imageData + y*pImgBG->widthStep;
299                 unsigned char* pFG = (unsigned char*)m_pImgFG->imageData +y*m_pImgFG->widthStep;
300
301                 for(x=0; x<xN; ++x)
302                 {
303                     if(pFG[x])
304                     {
305                         int D1 = (int)(pI[3*x+0])-(int)(pBG[3*x+0]);
306                         int D2 = (int)(pI[3*x+1])-(int)(pBG[3*x+1]);
307                         int D3 = (int)(pI[3*x+2])-(int)(pBG[3*x+2]);
308                         int DD = D1*D1+D2*D2+D3*D3;
309                         double  D = sqrt((float)DD);
310                         double  DW = 25;
311                         double  W = 1/(exp(-4*(D-m_BGImageUsing)/DW)+1);
312                         pFG[x] = (uchar)cvRound(W*255);
313                     }
314                 }   /* Next mask pixel. */
315             }   /*  Next mask line. */
316             /*if(m_Wnd)
317             {
318                 cvNamedWindow("BlobList_FGWeight",0);
319                 cvShowImage("BlobList_FGWeight",m_pImgFG);
320             }*/
321         }   /* Weighting mask mask. */
322
323         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
324         {   /* Predict position. */
325             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
326             CvBlob*         pB = pF->pPredictor->Predict();
327             if(pB)
328             {
329                 pF->BlobPredict = pB[0];
330                 pF->BlobPredict.w = pF->blob.w;
331                 pF->BlobPredict.h = pF->blob.h;
332             }
333         }   /* Predict position. */
334
335         if(m_Collision)
336         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
337         {   /* Predict collision. */
338             int             Collision = 0;
339             int             j;
340             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
341
342             for(j=m_BlobTrackerList.GetBlobNum(); j>0; --j)
343             {   /* Predict collision. */
344                 CvBlob* pB1;
345                 CvBlob* pB2;
346                 DefBlobTrackerL* pF2 = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(j-1);
347                 if(i==j) continue;
348                 pB1 = &pF->BlobPredict;
349                 pB2 = &pF2->BlobPredict;
350                 if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
351                     fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
352                 pB1 = &pF->blob;
353                 pB2 = &pF2->blob;
354                 if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
355                     fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
356                 if(Collision) break;
357             }   /* Check next blob to cross current. */
358
359             pF->Collision = Collision;
360             pF->pTracker->SetCollision(Collision);
361
362         }   /* Predict collision. */
363
364         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
365         {   /* Track each blob. */
366             DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
367             if(pF->pBlobHyp->GetBlobNum()>0)
368             {   /* Track all hypothesis. */
369                 int h,hN = pF->pBlobHyp->GetBlobNum();
370                 for(h=0;h<hN;++h)
371                 {
372                     CvBlob*     pB = pF->pBlobHyp->GetBlob(h);
373                     CvBlob*     pNewBlob = pF->pTracker->Process(pB,pImg,m_pImgFG);
374                     int         BlobID = CV_BLOB_ID(pB);
375                     if(pNewBlob)
376                     {
377                         pB[0] = pNewBlob[0];
378                         pB->w = MAX(CV_BLOB_MINW,pNewBlob->w);
379                         pB->h = MAX(CV_BLOB_MINH,pNewBlob->h);
380                         CV_BLOB_ID(pB) = BlobID;
381                     }
382                 }   /* Next hypothesis. */
383
384             }   /* Track all hypotheses. */
385
386             pF->Frame++;
387
388         }   /* Next blob. */
389
390 #if 0
391         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
392         {   /* Update predictor: */
393             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
394             if((m_Collision && !pF->Collision) || !m_Collision)
395             {
396                 pF->pPredictor->Update((CvBlob*)pF);
397             }
398             else
399             {   /* pravilnyp putem idete tovarischy!!! */
400                 pF->pPredictor->Update(&(pF->BlobPredict));
401             }
402         }   /* Update predictor. */
403 #endif
404         m_ClearHyp = 1;
405     };
406
407
408     /* Process on blob (for multi hypothesis tracing) */
409     virtual void ProcessBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
410     {
411         int                 ID = pBlob->ID;
412         DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
413         CvBlob*             pNewBlob = pF->pTracker->Process(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
414         if(pNewBlob)
415         {
416             pF->blob = pNewBlob[0];
417             pF->blob.w = MAX(CV_BLOB_MINW,pNewBlob->w);
418             pF->blob.h = MAX(CV_BLOB_MINH,pNewBlob->h);
419             pBlob[0] = pF->blob;
420         }
421         pBlob->ID = ID;
422     };
423
424     virtual double  GetConfidence(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
425     {
426         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
427         if(pF==NULL) return 0;
428         if(pF->pTracker==NULL) return 0;
429         return pF->pTracker->GetConfidence(pBlob?pBlob:(&pF->blob), pImg, pImgFG, NULL);
430     };
431
432     virtual double GetConfidenceList(CvBlobSeq* pBlobList, IplImage* pImg, IplImage* pImgFG = NULL)
433     {
434         double  W = 1;
435         int     b,bN = pBlobList->GetBlobNum();
436
437         if(m_pImgReg == NULL) 
438         {
439             m_pImgReg = cvCreateImage(cvSize(pImg->width,pImg->height),IPL_DEPTH_8U,1);
440         }
441         assert(pImg);
442
443         cvSet(m_pImgReg,cvScalar(255));
444
445         for(b=0; b<bN; ++b)
446         {
447             CvBlob* pB = pBlobList->GetBlob(b);
448             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(pB->ID);
449             if(pF==NULL || pF->pTracker==NULL) continue;
450             W *= pF->pTracker->GetConfidence(pB, pImg, pImgFG, m_pImgReg );
451             cvEllipse( 
452                 m_pImgReg, 
453                 cvPoint(cvRound(pB->x*256),cvRound(pB->y*256)), cvSize(cvRound(pB->w*128),cvRound(pB->h*128)), 
454                 0, 0, 360, 
455                 cvScalar(0), CV_FILLED, 8, 8 );
456 //            cvNamedWindow("REG",0);
457 //            cvShowImage("REG",m_pImgReg);
458 //            cvWaitKey(0);
459         }
460         return W;
461     };
462
463     virtual void UpdateBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
464     {
465         DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
466         if(pF)
467         {
468             pF->pTracker->Update(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
469         }
470     };
471
472     int     GetBlobNum(){return m_BlobTrackerList.GetBlobNum();};
473     CvBlob* GetBlob(int index){return m_BlobTrackerList.GetBlob(index);};
474
475     void  SetBlob(int BlobIndex, CvBlob* pBlob)
476     {
477         CvBlob* pB = m_BlobTrackerList.GetBlob(BlobIndex);
478         if(pB)
479         {
480             pB[0] = pBlob[0];
481             pB->w = MAX(CV_BLOB_MINW, pBlob->w);
482             pB->h = MAX(CV_BLOB_MINH, pBlob->h);
483         }
484     }
485
486     void    Release(){delete this;};
487
488     /* Additional functionality: */
489     CvBlob* GetBlobByID(int BlobID){return m_BlobTrackerList.GetBlobByID(BlobID);}
490
491     /*  ===============  MULTI HYPOTHESIS INTERFACE ==================  */
492     /* Return number of position hypotheses of currently tracked blob: */
493     virtual int     GetBlobHypNum(int BlobIdx)
494     {
495         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIdx);
496         assert(pF->pBlobHyp);
497         return pF->pBlobHyp->GetBlobNum();
498     };  /* CvBlobtrackerList::GetBlobHypNum() */
499
500     /* Return pointer to specified blob hypothesis by index blob: */
501     virtual CvBlob* GetBlobHyp(int BlobIndex, int hypothesis)
502     {
503         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
504         assert(pF->pBlobHyp);
505         return pF->pBlobHyp->GetBlob(hypothesis);
506     };  /* CvBlobtrackerList::GetBlobHyp() */
507
508     /* Set new parameters for specified (by index) blob hyp (can be called several times for each hyp )*/
509     virtual void    SetBlobHyp(int BlobIndex, CvBlob* pBlob)
510     {
511         if(m_ClearHyp)
512         {   /* Clear all hypotheses: */
513             int b, bN = m_BlobTrackerList.GetBlobNum();
514             for(b=0; b<bN; ++b)
515             {
516                 DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(b);
517                 assert(pF->pBlobHyp);
518                 pF->pBlobHyp->Clear();
519             }
520             m_ClearHyp = 0;
521         }
522         {   /* Add hypothesis: */
523             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
524             assert(pF->pBlobHyp);
525             pF->pBlobHyp->AddBlob(pBlob);
526         }
527     };  /* CvBlobtrackerList::SetBlobHyp */
528
529 private:
530 public:
531     void ParamUpdate()
532     {
533         int i;
534         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
535         {
536             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
537             TransferParamsToChild(pF->pTracker);
538             pF->pTracker->ParamUpdate();
539         }
540     }
541 };  /* CvBlobTrackerList */
542
543 CvBlobTracker* cvCreateBlobTrackerList(CvBlobTrackerOne* (*create)())
544 {
545     return (CvBlobTracker*) new CvBlobTrackerList(create);
546 }