Move the sources to trunk
[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 typedef struct CvPixHist{
56     CvPixHistBin   bins[PIX_HIST_BIN_NUM_2];
57 }CvPixHist;
58 //class for background statistics estimation
59 class CvBGEstimPixHist
60 {
61 private:
62     CvPixHist*  m_PixHists;
63     int         m_width;
64     int         m_height;
65
66     //function for update color histogram for one pixel
67     void update_hist_elem(int x, int y, uchar* cols )
68     {
69         //find closest bin
70         int    dist = 0, min_dist = 2147483647, indx = -1;
71         for( int k = 0; k < PIX_HIST_BIN_NUM_2; k++ ){
72             uchar* hist_cols = m_PixHists[y*m_width+x].bins[k].cols;
73             m_PixHists[y*m_width+x].bins[k].bin_val *= (1-PIX_HIST_ALPHA);
74             int l;
75             for( l = 0; l < 3; l++ ){
76                 int val = abs( hist_cols[l] - cols[l] );
77                 if( val > PIX_HIST_DELTA_IN_PIX_VAL ) break;
78                 dist += val;
79             }
80             if( l == 3 && dist < min_dist ){
81                 min_dist = dist;
82                 indx = k;
83             }
84         }
85         if( indx < 0 ){//N2th elem in the table is replaced by a new features
86             indx = PIX_HIST_BIN_NUM_2 - 1;
87             m_PixHists[y*m_width+x].bins[indx].bin_val = PIX_HIST_ALPHA;
88             for(int l = 0; l < 3; l++ ){
89                 m_PixHists[y*m_width+x].bins[indx].cols[l] = cols[l];
90             }
91         }
92         else {
93             //add vote!
94             m_PixHists[y*m_width+x].bins[indx].bin_val += PIX_HIST_ALPHA;
95         }
96         //re-sort bins by BIN_VAL
97         {
98             int k;
99             for(k = 0; k < indx; k++ ){
100                 if( m_PixHists[y*m_width+x].bins[k].bin_val <= m_PixHists[y*m_width+x].bins[indx].bin_val ){
101                     CvPixHistBin tmp1, tmp2 = m_PixHists[y*m_width+x].bins[indx];
102                     //shift elements
103                     for(int l = k; l <= indx; l++ ){
104                         tmp1 = m_PixHists[y*m_width+x].bins[l];
105                         m_PixHists[y*m_width+x].bins[l] = tmp2;
106                         tmp2 = tmp1;
107                     }
108                     break;
109                 }
110             }
111         }
112     }//void update_hist(...)
113
114     //function for calculation difference between histograms
115     float get_hist_diff(int x1, int y1, int x2, int y2)
116     {
117         float  dist = 0;
118         for( int i = 0; i < 3; i++ ){
119             dist += labs(m_PixHists[y1*m_width+x1].bins[0].cols[i] -
120                                 m_PixHists[y2*m_width+x2].bins[0].cols[i]);
121         }
122         return dist;
123     }
124
125
126 public:
127     IplImage*   bg_image;
128
129     CvBGEstimPixHist(CvSize img_size)
130     {
131         m_PixHists = (CvPixHist*)cvAlloc(img_size.width*img_size.height*sizeof(CvPixHist));
132         memset( m_PixHists, 0, img_size.width*img_size.height*sizeof(CvPixHist) );
133         m_width = img_size.width;
134         m_height = img_size.height;
135
136         bg_image = cvCreateImage(img_size, IPL_DEPTH_8U, 3 );
137     }/* constructor */
138
139     ~CvBGEstimPixHist()
140     {
141         cvReleaseImage(&bg_image);
142         cvFree(&m_PixHists);
143     }/* destructor */
144
145
146     //function for update histograms and bg_image
147     void update_hists( IplImage* pImg )
148     {
149         for( int i = 0; i < pImg->height; i++ ){
150             for( int j = 0; j < pImg->width; j++ ){
151                 update_hist_elem( j, i, ((uchar*)(pImg->imageData))+i*pImg->widthStep+3*j );
152                 ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j] = m_PixHists[i*m_width+j].bins[0].cols[0];
153                 ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j+1] = m_PixHists[i*m_width+j].bins[0].cols[1];
154                 ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j+2] = m_PixHists[i*m_width+j].bins[0].cols[2];
155             }
156         }
157         //cvNamedWindow("RoadMap2",0);
158         //cvShowImage("RoadMap2", bg_image);
159     }
160 };/* CvBGEstimPixHist */
161
162
163
164 /*======================= TRACKER LIST SHELL =====================*/
165 typedef struct DefBlobTrackerL
166 {
167     CvBlob                  blob;
168     CvBlobTrackerOne*       pTracker;
169     int                     Frame;
170     int                     Collision;
171     CvBlobTrackPredictor*   pPredictor;
172     CvBlob                  BlobPredict;
173     CvBlobSeq*              pBlobHyp;
174 } DefBlobTrackerL;
175
176 class CvBlobTrackerList : public CvBlobTracker
177 {
178 private:
179     CvBlobTrackerOne*       (*m_Create)();
180     CvBlobSeq               m_BlobTrackerList;
181 //    int                     m_LastID;
182     int                     m_Collision;
183     int                     m_ClearHyp;
184     float                   m_BGImageUsing;
185     CvBGEstimPixHist*       m_pBGImage;
186     IplImage*               m_pImgFG;
187     IplImage*               m_pImgReg; /* mask for multiblob confidence calculation */
188 public:
189     CvBlobTrackerList(CvBlobTrackerOne* (*create)()):m_BlobTrackerList(sizeof(DefBlobTrackerL))
190     {
191         //int i;
192         CvBlobTrackerOne* pM = create();
193 //        m_LastID = 0;
194         m_Create = create;
195         m_ClearHyp = 0;
196         m_pImgFG = 0;
197         m_pImgReg = NULL;
198
199         TransferParamsFromChild(pM,NULL);
200
201         pM->Release();
202
203         m_Collision = 1; /* if 1 then collistion will be detected and processed */
204         AddParam("Collision",&m_Collision);
205         CommentParam("Collision", "if 1 then collision cases are processed in special way");
206
207         m_pBGImage = NULL;
208         m_BGImageUsing = 50;
209         AddParam("BGImageUsing", &m_BGImageUsing);
210         CommentParam("BGImageUsing","Weight of using BG image in update hist model (0 - BG dies not use 1 - use)");
211     }
212     ~CvBlobTrackerList()
213     {
214         int i;
215         if(m_pBGImage) delete m_pBGImage;
216         if(m_pImgFG) cvReleaseImage(&m_pImgFG);
217         if(m_pImgReg) cvReleaseImage(&m_pImgReg);
218         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
219         {
220             m_BlobTrackerList.DelBlob(i-1);
221         }
222     };
223     CvBlob* AddBlob(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG )
224     {/* create new tracker */
225         DefBlobTrackerL F;
226         F.blob = pBlob[0];
227 //        F.blob.ID = m_LastID++;
228         F.pTracker = m_Create();
229         F.pPredictor = cvCreateModuleBlobTrackPredictKalman();
230         F.pBlobHyp = new CvBlobSeq;
231         F.Frame = 0;
232         TransferParamsToChild(F.pTracker,NULL);
233
234         F.pTracker->Init(pBlob,pImg, pImgFG);
235         m_BlobTrackerList.AddBlob((CvBlob*)&F);
236         return m_BlobTrackerList.GetBlob(m_BlobTrackerList.GetBlobNum()-1);
237     };
238     void DelBlob(int BlobIndex)
239     {
240         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
241         if(pF == NULL) return;
242         pF->pTracker->Release();
243         pF->pPredictor->Release();
244         delete pF->pBlobHyp;
245         m_BlobTrackerList.DelBlob(BlobIndex);
246     }
247     void DelBlobByID(int BlobID)
248     {
249         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(BlobID);
250         if(pF == NULL) return;
251         pF->pTracker->Release();
252         pF->pPredictor->Release();
253         delete pF->pBlobHyp;
254         m_BlobTrackerList.DelBlobByID(BlobID);
255     }
256
257     virtual void Process(IplImage* pImg, IplImage* pImgFG = NULL)
258     {
259         int i;
260         if(pImgFG)
261         {
262             if(m_pImgFG) cvCopyImage(pImgFG,m_pImgFG);
263             else m_pImgFG = cvCloneImage(pImgFG);
264         }
265
266         if(m_pBGImage==NULL && m_BGImageUsing>0)
267         {
268             m_pBGImage = new CvBGEstimPixHist(cvSize(pImg->width,pImg->height));
269         }
270
271         if(m_Collision)
272         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
273         {/* update predictor */
274             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
275             pF->pPredictor->Update((CvBlob*)pF);
276         }/* update predictor */
277
278         if(m_pBGImage && m_pImgFG)
279         {/* wheighting mask mask */
280             int x,y,yN=pImg->height,xN=pImg->width;
281             IplImage* pImgBG = NULL;
282             m_pBGImage->update_hists(pImg);
283             pImgBG = m_pBGImage->bg_image;
284             for(y=0;y<yN;++y)
285             {
286                 unsigned char* pI = (unsigned char*)pImg->imageData + y*pImg->widthStep;
287                 unsigned char* pBG = (unsigned char*)pImgBG->imageData + y*pImgBG->widthStep;
288                 unsigned char* pFG = (unsigned char*)m_pImgFG->imageData +y*m_pImgFG->widthStep;
289                 for(x=0;x<xN;++x)
290                 {
291                     if(pFG[x])
292                     {
293                         int D1 = (int)(pI[3*x+0])-(int)(pBG[3*x+0]);
294                         int D2 = (int)(pI[3*x+1])-(int)(pBG[3*x+1]);
295                         int D3 = (int)(pI[3*x+2])-(int)(pBG[3*x+2]);
296                         int DD = D1*D1+D2*D2+D3*D3;
297                         double  D = sqrt((float)DD);
298                         double  DW = 25;
299                         double  W = 1/(exp(-4*(D-m_BGImageUsing)/DW)+1);
300                         pFG[x] = (uchar)cvRound(W*255);
301                     }
302                 }/* next mask pixel */
303             }/* next mask line */
304             /*if(m_Wnd)
305             {
306                 cvNamedWindow("BlobList_FGWeight",0);
307                 cvShowImage("BlobList_FGWeight",m_pImgFG);
308             }*/
309         }/* wheighting mask mask */
310
311         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
312         {/* predict position */
313             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
314             CvBlob*         pB = pF->pPredictor->Predict();
315             if(pB)
316             {
317                 pF->BlobPredict = pB[0];
318                 pF->BlobPredict.w = pF->blob.w;
319                 pF->BlobPredict.h = pF->blob.h;
320             }
321         }/* predict position */
322
323         if(m_Collision)
324         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
325         {/* predict collision */
326             int             Collision = 0;
327             int             j;
328             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
329             for(j=m_BlobTrackerList.GetBlobNum();j>0;--j)
330             {/* predict collision */
331                 CvBlob* pB1;
332                 CvBlob* pB2;
333                 DefBlobTrackerL* pF2 = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(j-1);
334                 if(i==j) continue;
335                 pB1 = &pF->BlobPredict;
336                 pB2 = &pF2->BlobPredict;
337                 if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
338                     fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
339                 pB1 = &pF->blob;
340                 pB2 = &pF2->blob;
341                 if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
342                     fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
343                 if(Collision) break;
344             }/* check next blob to cross current*/
345             pF->Collision = Collision;
346             pF->pTracker->SetCollision(Collision);
347         }/* predict collision */
348
349         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
350         {/* track each blob */
351             DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
352             if(pF->pBlobHyp->GetBlobNum()>0)
353             {/* track all hypothesis */
354                 int h,hN = pF->pBlobHyp->GetBlobNum();
355                 for(h=0;h<hN;++h)
356                 {
357                     CvBlob*     pB = pF->pBlobHyp->GetBlob(h);
358                     CvBlob*     pNewBlob = pF->pTracker->Process(pB,pImg,m_pImgFG);
359                     int         BlobID = CV_BLOB_ID(pB);
360                     if(pNewBlob)
361                     {
362                         pB[0] = pNewBlob[0];
363                         pB->w = MAX(CV_BLOB_MINW,pNewBlob->w);
364                         pB->h = MAX(CV_BLOB_MINH,pNewBlob->h);
365                         CV_BLOB_ID(pB) = BlobID;
366                     }
367                 }/* next hyp*/
368
369             }/* track all hypothesis */
370             pF->Frame++;
371         }/* next blob */
372
373 #if 0
374         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
375         {/* update predictor */
376             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
377             if((m_Collision && !pF->Collision) || !m_Collision)
378             {
379                 pF->pPredictor->Update((CvBlob*)pF);
380             }
381             else
382             {/* pravilnyp putem idete tovarischy!!! */
383                 pF->pPredictor->Update(&(pF->BlobPredict));
384             }
385         }/* update predictor */
386 #endif
387         m_ClearHyp = 1;
388     };
389
390
391     /* Process on blob (for multi hypothesis tracing) */
392     virtual void ProcessBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
393     {
394         int                 ID = pBlob->ID;
395         DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
396         CvBlob*             pNewBlob = pF->pTracker->Process(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
397         if(pNewBlob)
398         {
399             pF->blob = pNewBlob[0];
400             pF->blob.w = MAX(CV_BLOB_MINW,pNewBlob->w);
401             pF->blob.h = MAX(CV_BLOB_MINH,pNewBlob->h);
402             pBlob[0] = pF->blob;
403         }
404         pBlob->ID = ID;
405     };
406     virtual double  GetConfidence(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
407     {
408         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
409         if(pF==NULL) return 0;
410         if(pF->pTracker==NULL) return 0;
411         return pF->pTracker->GetConfidence(pBlob?pBlob:(&pF->blob), pImg, pImgFG, NULL);
412     };
413     virtual double GetConfidenceList(CvBlobSeq* pBlobList, IplImage* pImg, IplImage* pImgFG = NULL)
414     {
415         double  W = 1;
416         int     b,bN = pBlobList->GetBlobNum();
417
418         if(m_pImgReg == NULL) 
419         {
420             m_pImgReg = cvCreateImage(cvSize(pImg->width,pImg->height),IPL_DEPTH_8U,1);
421         }
422         assert(pImg);
423
424         cvSet(m_pImgReg,cvScalar(255));
425
426         for(b=0;b<bN;++b)
427         {
428             CvBlob* pB = pBlobList->GetBlob(b);
429             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(pB->ID);
430             if(pF==NULL || pF->pTracker==NULL) continue;
431             W *= pF->pTracker->GetConfidence(pB, pImg, pImgFG, m_pImgReg );
432             cvEllipse( 
433                 m_pImgReg, 
434                 cvPoint(cvRound(pB->x*256),cvRound(pB->y*256)), cvSize(cvRound(pB->w*128),cvRound(pB->h*128)), 
435                 0, 0, 360, 
436                 cvScalar(0), CV_FILLED, 8, 8 );
437 //            cvNamedWindow("REG",0);
438 //            cvShowImage("REG",m_pImgReg);
439 //            cvWaitKey(0);
440         }
441         return W;
442     };
443     virtual void UpdateBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
444     {
445         DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
446         if(pF)
447         {
448             pF->pTracker->Update(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
449         }
450     };
451
452     int     GetBlobNum(){return m_BlobTrackerList.GetBlobNum();};
453     CvBlob* GetBlob(int index){return m_BlobTrackerList.GetBlob(index);};
454     void  SetBlob(int BlobIndex, CvBlob* pBlob)
455     {
456         CvBlob* pB = m_BlobTrackerList.GetBlob(BlobIndex);
457         if(pB)
458         {
459             pB[0] = pBlob[0];
460             pB->w = MAX(CV_BLOB_MINW, pBlob->w);
461             pB->h = MAX(CV_BLOB_MINH, pBlob->h);
462         }
463     }
464
465     void    Release(){delete this;};
466
467     /* additional functionality */
468     CvBlob* GetBlobByID(int BlobID){return m_BlobTrackerList.GetBlobByID(BlobID);}
469
470     /*  ===============  MULTI HYPOTHESIS INTERFACE ==================  */
471     /* return number of position hyposetis of currently tracked blob */
472     virtual int     GetBlobHypNum(int BlobIdx)
473     {
474         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIdx);
475         assert(pF->pBlobHyp);
476         return pF->pBlobHyp->GetBlobNum();
477     };/* CvBlobtrackerList::GetBlobHypNum() */
478
479     /* return pointer to specified blob hypothesis by index blob */
480     virtual CvBlob* GetBlobHyp(int BlobIndex, int hypothesis)
481     {
482         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
483         assert(pF->pBlobHyp);
484         return pF->pBlobHyp->GetBlob(hypothesis);
485     };/* CvBlobtrackerList::GetBlobHyp() */
486     /* Set new parameters for specified (by index) blob hyp (can be called several times for each hyp )*/
487     virtual void    SetBlobHyp(int BlobIndex, CvBlob* pBlob)
488     {
489         if(m_ClearHyp)
490         {/* clear all hypothesis */
491             int b, bN = m_BlobTrackerList.GetBlobNum();
492             for(b=0;b<bN;++b)
493             {
494                 DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(b);
495                 assert(pF->pBlobHyp);
496                 pF->pBlobHyp->Clear();
497             }
498             m_ClearHyp = 0;
499         }
500         {/* add hypothesis */
501             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
502             assert(pF->pBlobHyp);
503             pF->pBlobHyp->AddBlob(pBlob);
504         }
505     }; /*CvBlobtrackerList::SetBlobHyp*/
506
507 private:
508 public:
509     void ParamUpdate()
510     {
511         int i;
512         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
513         {
514             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
515             TransferParamsToChild(pF->pTracker);
516             pF->pTracker->ParamUpdate();
517         }
518     }
519 }; /* CvBlobTrackerList */
520
521 CvBlobTracker* cvCreateBlobTrackerList(CvBlobTrackerOne* (*create)())
522 {
523     return (CvBlobTracker*) new CvBlobTrackerList(create);
524 }