Update to 2.0.0 tree from current Fremantle build
[opencv] / src / cvaux / 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         SetModuleName("List");
219     }
220
221     ~CvBlobTrackerList()
222     {
223         int i;
224         if(m_pBGImage) delete m_pBGImage;
225         if(m_pImgFG) cvReleaseImage(&m_pImgFG);
226         if(m_pImgReg) cvReleaseImage(&m_pImgReg);
227         for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
228         {
229             m_BlobTrackerList.DelBlob(i-1);
230         }
231     };
232
233     CvBlob* AddBlob(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG )
234     {   /* Create new tracker: */
235         DefBlobTrackerL F;
236         F.blob = pBlob[0];
237 //        F.blob.ID = m_LastID++;
238         F.pTracker = m_Create();
239         F.pPredictor = cvCreateModuleBlobTrackPredictKalman();
240         F.pBlobHyp = new CvBlobSeq;
241         F.Frame = 0;
242         TransferParamsToChild(F.pTracker,NULL);
243
244         F.pTracker->Init(pBlob,pImg, pImgFG);
245         m_BlobTrackerList.AddBlob((CvBlob*)&F);
246         return m_BlobTrackerList.GetBlob(m_BlobTrackerList.GetBlobNum()-1);
247     };
248
249     void DelBlob(int BlobIndex)
250     {
251         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
252         if(pF == NULL) return;
253         pF->pTracker->Release();
254         pF->pPredictor->Release();
255         delete pF->pBlobHyp;
256         m_BlobTrackerList.DelBlob(BlobIndex);
257     }
258
259     void DelBlobByID(int BlobID)
260     {
261         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(BlobID);
262         if(pF == NULL) return;
263         pF->pTracker->Release();
264         pF->pPredictor->Release();
265         delete pF->pBlobHyp;
266         m_BlobTrackerList.DelBlobByID(BlobID);
267     }
268
269     virtual void Process(IplImage* pImg, IplImage* pImgFG = NULL)
270     {
271         int i;
272         if(pImgFG)
273         {
274             if(m_pImgFG) cvCopyImage(pImgFG,m_pImgFG);
275             else         m_pImgFG = cvCloneImage(pImgFG);
276         }
277
278         if(m_pBGImage==NULL && m_BGImageUsing>0)
279         {
280             m_pBGImage = new CvBGEstimPixHist(cvSize(pImg->width,pImg->height));
281         }
282
283         if(m_Collision)
284         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
285         {   /* Update predictor: */
286             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
287             pF->pPredictor->Update((CvBlob*)pF);
288         }   /* Update predictor. */
289
290         if(m_pBGImage && m_pImgFG)
291         {   /* Weighting mask mask: */
292             int x,y,yN=pImg->height,xN=pImg->width;
293             IplImage* pImgBG = NULL;
294             m_pBGImage->update_hists(pImg);
295             pImgBG = m_pBGImage->bg_image;
296
297             for(y=0; y<yN; ++y)
298             {
299                 unsigned char* pI = (unsigned char*)pImg->imageData + y*pImg->widthStep;
300                 unsigned char* pBG = (unsigned char*)pImgBG->imageData + y*pImgBG->widthStep;
301                 unsigned char* pFG = (unsigned char*)m_pImgFG->imageData +y*m_pImgFG->widthStep;
302
303                 for(x=0; x<xN; ++x)
304                 {
305                     if(pFG[x])
306                     {
307                         int D1 = (int)(pI[3*x+0])-(int)(pBG[3*x+0]);
308                         int D2 = (int)(pI[3*x+1])-(int)(pBG[3*x+1]);
309                         int D3 = (int)(pI[3*x+2])-(int)(pBG[3*x+2]);
310                         int DD = D1*D1+D2*D2+D3*D3;
311                         double  D = sqrt((float)DD);
312                         double  DW = 25;
313                         double  W = 1/(exp(-4*(D-m_BGImageUsing)/DW)+1);
314                         pFG[x] = (uchar)cvRound(W*255);
315                     }
316                 }   /* Next mask pixel. */
317             }   /*  Next mask line. */
318             /*if(m_Wnd)
319             {
320                 cvNamedWindow("BlobList_FGWeight",0);
321                 cvShowImage("BlobList_FGWeight",m_pImgFG);
322             }*/
323         }   /* Weighting mask mask. */
324
325         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
326         {   /* Predict position. */
327             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
328             CvBlob*         pB = pF->pPredictor->Predict();
329             if(pB)
330             {
331                 pF->BlobPredict = pB[0];
332                 pF->BlobPredict.w = pF->blob.w;
333                 pF->BlobPredict.h = pF->blob.h;
334             }
335         }   /* Predict position. */
336
337         if(m_Collision)
338         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
339         {   /* Predict collision. */
340             int             Collision = 0;
341             int             j;
342             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
343
344             for(j=m_BlobTrackerList.GetBlobNum(); j>0; --j)
345             {   /* Predict collision. */
346                 CvBlob* pB1;
347                 CvBlob* pB2;
348                 DefBlobTrackerL* pF2 = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(j-1);
349                 if(i==j) continue;
350                 pB1 = &pF->BlobPredict;
351                 pB2 = &pF2->BlobPredict;
352                 if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
353                     fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
354                 pB1 = &pF->blob;
355                 pB2 = &pF2->blob;
356                 if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
357                     fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
358                 if(Collision) break;
359             }   /* Check next blob to cross current. */
360
361             pF->Collision = Collision;
362             pF->pTracker->SetCollision(Collision);
363
364         }   /* Predict collision. */
365
366         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
367         {   /* Track each blob. */
368             DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
369             if(pF->pBlobHyp->GetBlobNum()>0)
370             {   /* Track all hypothesis. */
371                 int h,hN = pF->pBlobHyp->GetBlobNum();
372                 for(h=0;h<hN;++h)
373                 {
374                     CvBlob*     pB = pF->pBlobHyp->GetBlob(h);
375                     CvBlob*     pNewBlob = pF->pTracker->Process(pB,pImg,m_pImgFG);
376                     int         BlobID = CV_BLOB_ID(pB);
377                     if(pNewBlob)
378                     {
379                         pB[0] = pNewBlob[0];
380                         pB->w = MAX(CV_BLOB_MINW,pNewBlob->w);
381                         pB->h = MAX(CV_BLOB_MINH,pNewBlob->h);
382                         CV_BLOB_ID(pB) = BlobID;
383                     }
384                 }   /* Next hypothesis. */
385
386             }   /* Track all hypotheses. */
387
388             pF->Frame++;
389
390         }   /* Next blob. */
391
392 #if 0
393         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
394         {   /* Update predictor: */
395             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
396             if((m_Collision && !pF->Collision) || !m_Collision)
397             {
398                 pF->pPredictor->Update((CvBlob*)pF);
399             }
400             else
401             {   /* pravilnyp putem idete tovarischy!!! */
402                 pF->pPredictor->Update(&(pF->BlobPredict));
403             }
404         }   /* Update predictor. */
405 #endif
406         m_ClearHyp = 1;
407     };
408
409
410     /* Process on blob (for multi hypothesis tracing) */
411     virtual void ProcessBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
412     {
413         int                 ID = pBlob->ID;
414         DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
415         CvBlob*             pNewBlob = pF->pTracker->Process(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
416         if(pNewBlob)
417         {
418             pF->blob = pNewBlob[0];
419             pF->blob.w = MAX(CV_BLOB_MINW,pNewBlob->w);
420             pF->blob.h = MAX(CV_BLOB_MINH,pNewBlob->h);
421             pBlob[0] = pF->blob;
422         }
423         pBlob->ID = ID;
424     };
425
426     virtual double  GetConfidence(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
427     {
428         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
429         if(pF==NULL) return 0;
430         if(pF->pTracker==NULL) return 0;
431         return pF->pTracker->GetConfidence(pBlob?pBlob:(&pF->blob), pImg, pImgFG, NULL);
432     };
433
434     virtual double GetConfidenceList(CvBlobSeq* pBlobList, IplImage* pImg, IplImage* pImgFG = NULL)
435     {
436         double  W = 1;
437         int     b,bN = pBlobList->GetBlobNum();
438
439         if(m_pImgReg == NULL)
440         {
441             m_pImgReg = cvCreateImage(cvSize(pImg->width,pImg->height),IPL_DEPTH_8U,1);
442         }
443         assert(pImg);
444
445         cvSet(m_pImgReg,cvScalar(255));
446
447         for(b=0; b<bN; ++b)
448         {
449             CvBlob* pB = pBlobList->GetBlob(b);
450             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(pB->ID);
451             if(pF==NULL || pF->pTracker==NULL) continue;
452             W *= pF->pTracker->GetConfidence(pB, pImg, pImgFG, m_pImgReg );
453             cvEllipse(
454                 m_pImgReg,
455                 cvPoint(cvRound(pB->x*256),cvRound(pB->y*256)), cvSize(cvRound(pB->w*128),cvRound(pB->h*128)),
456                 0, 0, 360,
457                 cvScalar(0), CV_FILLED, 8, 8 );
458 //            cvNamedWindow("REG",0);
459 //            cvShowImage("REG",m_pImgReg);
460 //            cvWaitKey(0);
461         }
462         return W;
463     };
464
465     virtual void UpdateBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
466     {
467         DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
468         if(pF)
469         {
470             pF->pTracker->Update(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
471         }
472     };
473
474     int     GetBlobNum(){return m_BlobTrackerList.GetBlobNum();};
475     CvBlob* GetBlob(int index){return m_BlobTrackerList.GetBlob(index);};
476
477     void  SetBlob(int BlobIndex, CvBlob* pBlob)
478     {
479         CvBlob* pB = m_BlobTrackerList.GetBlob(BlobIndex);
480         if(pB)
481         {
482             pB[0] = pBlob[0];
483             pB->w = MAX(CV_BLOB_MINW, pBlob->w);
484             pB->h = MAX(CV_BLOB_MINH, pBlob->h);
485         }
486     }
487
488     void    Release(){delete this;};
489
490     /* Additional functionality: */
491     CvBlob* GetBlobByID(int BlobID){return m_BlobTrackerList.GetBlobByID(BlobID);}
492
493     /*  ===============  MULTI HYPOTHESIS INTERFACE ==================  */
494     /* Return number of position hypotheses of currently tracked blob: */
495     virtual int     GetBlobHypNum(int BlobIdx)
496     {
497         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIdx);
498         assert(pF->pBlobHyp);
499         return pF->pBlobHyp->GetBlobNum();
500     };  /* CvBlobtrackerList::GetBlobHypNum() */
501
502     /* Return pointer to specified blob hypothesis by index blob: */
503     virtual CvBlob* GetBlobHyp(int BlobIndex, int hypothesis)
504     {
505         DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
506         assert(pF->pBlobHyp);
507         return pF->pBlobHyp->GetBlob(hypothesis);
508     };  /* CvBlobtrackerList::GetBlobHyp() */
509
510     /* Set new parameters for specified (by index) blob hyp (can be called several times for each hyp )*/
511     virtual void    SetBlobHyp(int BlobIndex, CvBlob* pBlob)
512     {
513         if(m_ClearHyp)
514         {   /* Clear all hypotheses: */
515             int b, bN = m_BlobTrackerList.GetBlobNum();
516             for(b=0; b<bN; ++b)
517             {
518                 DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(b);
519                 assert(pF->pBlobHyp);
520                 pF->pBlobHyp->Clear();
521             }
522             m_ClearHyp = 0;
523         }
524         {   /* Add hypothesis: */
525             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
526             assert(pF->pBlobHyp);
527             pF->pBlobHyp->AddBlob(pBlob);
528         }
529     };  /* CvBlobtrackerList::SetBlobHyp */
530
531 private:
532 public:
533     void ParamUpdate()
534     {
535         int i;
536         for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
537         {
538             DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
539             TransferParamsToChild(pF->pTracker);
540             pF->pTracker->ParamUpdate();
541         }
542     }
543 };  /* CvBlobTrackerList */
544
545 CvBlobTracker* cvCreateBlobTrackerList(CvBlobTrackerOne* (*create)())
546 {
547     return (CvBlobTracker*) new CvBlobTrackerList(create);
548 }