1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
12 // Copyright (C) 2000, Intel Corporation, all rights reserved.
13 // Third party copyrights are property of their respective owners.
15 // Redistribution and use in source and binary forms, with or without modification,
16 // are permitted provided that the following conditions are met:
18 // * Redistribution's of source code must retain the above copyright notice,
19 // this list of conditions and the following disclaimer.
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.
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.
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.
41 #include "precomp.hpp"
43 /* Blob (foreground-pixel connected-component) tracking with collision resolution.
45 * For entrypoints into the literature see:
47 * A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking
48 * Arulampalam &t al, 2001, 15p
49 * http://www-clmc.usc.edu/publications/A/arulampalam-TSP2002.pdf
51 * Particle Filters for Positioning, Navigation, and Tracking
52 * Gustafsson et al, 2002 12p
53 * http://www.control.isy.liu.se/~fredrik/reports/01SPpf4pos.pdf
55 * Particle Filtering in High Clutter Environments
56 * Korhonen et al, 2005 4p
57 * http://www.cs.uku.fi/finsig05/papers/paper26_FINSIG05.pdf
59 * Appearance Models for Occlusion Handling
60 * Andrew Senior &t al, 8p 2001
61 * http://www.research.ibm.com/peoplevision/PETS2001.pdf
65 /*============== BLOB TRACKERCC CLASS DECLARATION =============== */
66 typedef struct DefBlobTrackerCR
69 CvBlobTrackPredictor* pPredictor;
74 CvBlobTrackerOne* pResolver;
77 void cvFindBlobsByCCClasters(IplImage* pFG, CvBlobSeq* pBlobs, CvMemStorage* storage);
79 class CvBlobTrackerCCCR : public CvBlobTracker
85 CvBlobSeq m_BlobListNew;
87 CvBlobTrackerOne* (*m_CreateCR)();
88 char m_ModuleName[1024];
92 CvBlobTrackerCCCR(CvBlobTrackerOne* (*CreateCR)(), const char* CRName):m_BlobList(sizeof(DefBlobTrackerCR))
94 m_CreateCR = CreateCR;
95 m_pMem = cvCreateMemStorage();
97 m_Collision = 1; /* if 1 then collistion will be detected and processed */
100 AddParam("AlphaSize",&m_AlphaSize);
101 CommentParam("AlphaSize", "Size update speed (0..1)");
103 strcpy(m_ModuleName, "CCCR");
104 if(CRName)strcat(m_ModuleName,CRName);
105 SetModuleName(m_ModuleName);
108 CvBlobTrackerOne* pM = m_CreateCR();
109 TransferParamsFromChild(pM,NULL);
112 SetParam("SizeVar",0);
117 if(m_pMem)cvReleaseMemStorage(&m_pMem);
120 /* Blob functions: */
121 virtual int GetBlobNum() {return m_BlobList.GetBlobNum();}
122 virtual CvBlob* GetBlob(int BlobIndex){return m_BlobList.GetBlob(BlobIndex);}
123 virtual void SetBlob(int BlobIndex, CvBlob* pBlob)
125 CvBlob* pB = m_BlobList.GetBlob(BlobIndex);
126 if(pB) pB[0] = pBlob[0];
129 virtual CvBlob* GetBlobByID(int BlobID){return m_BlobList.GetBlobByID(BlobID);}
130 virtual void DelBlob(int BlobIndex)
132 DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(BlobIndex);
133 if(pBT->pResolver)pBT->pResolver->Release();
134 if(pBT->pPredictor)pBT->pPredictor->Release();
135 delete pBT->pBlobHyp;
136 m_BlobList.DelBlob(BlobIndex);
139 virtual void DelBlobByID(int BlobID)
141 DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(BlobID);
142 if(pBT->pResolver)pBT->pResolver->Release();
143 if(pBT->pPredictor)pBT->pPredictor->Release();
144 delete pBT->pBlobHyp;
145 m_BlobList.DelBlobByID(BlobID);
148 virtual void Release(){delete this;}
150 /* Add new blob to track it and assign to this blob personal ID */
151 /* pBlob - pinter to structure with blob parameters (ID is ignored)*/
152 /* pImg - current image */
153 /* pImgFG - current foreground mask */
154 /* Return pointer to new added blob: */
155 virtual CvBlob* AddBlob(CvBlob* pB, IplImage* pImg, IplImage* pImgFG = NULL )
157 DefBlobTrackerCR NewB;
159 NewB.pBlobHyp = new CvBlobSeq;
160 NewB.pPredictor = cvCreateModuleBlobTrackPredictKalman(); /* module for predict position */
161 NewB.pPredictor->SetParam("DataNoisePos",0.001);
162 NewB.pPredictor->ParamUpdate();
163 NewB.pResolver = NULL;
166 NewB.pResolver = m_CreateCR();
167 TransferParamsToChild(NewB.pResolver,NULL);
168 NewB.pResolver->Init(pB, pImg, pImgFG);
170 m_BlobList.AddBlob((CvBlob*)&NewB);
171 return m_BlobList.GetBlob(m_BlobList.GetBlobNum()-1);
174 virtual void Process(IplImage* pImg, IplImage* pImgFG = NULL)
180 if(m_BlobList.GetBlobNum() <= 0 ) return;
182 /* Clear blob list for new blobs: */
183 m_BlobListNew.Clear();
186 cvClearMemStorage(m_pMem);
189 { /* One contour - one blob: */
190 IplImage* pBin = cvCloneImage(pImgFG);
192 cvThreshold(pBin,pBin,128,255,CV_THRESH_BINARY);
193 cvFindContours(pBin, m_pMem, &cnts, sizeof(CvContour), CV_RETR_EXTERNAL);
195 /* Process each contour: */
196 for(cnt = cnts; cnt; cnt=cnt->h_next)
201 double M00,X,Y,XX,YY;
203 CvRect r = ((CvContour*)cnt)->rect;
205 if(r.height < 3 || r.width < 3) continue;
206 cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
207 M00 = cvGetSpatialMoment( &m, 0, 0 );
208 if(M00 <= 0 ) continue;
209 X = cvGetSpatialMoment( &m, 1, 0 )/M00;
210 Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
211 XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
212 YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
213 NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));
214 m_BlobListNew.AddBlob(&NewBlob);
216 } /* Next contour. */
218 cvReleaseImage(&pBin);
221 for(int i=m_BlobList.GetBlobNum(); i>0; --i)
222 { /* Predict new blob position. */
224 DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
226 /* Update predictor. */
227 pBT->pPredictor->Update(&(pBT->blob));
228 pB = pBT->pPredictor->Predict();
231 pBT->BlobPredict = pB[0];
233 pBT->BlobPrev = pBT->blob;
234 } /* Predict new blob position. */
237 if(m_BlobList.GetBlobNum()>0 && m_BlobListNew.GetBlobNum()>0)
238 { /* Resolve new blob to old: */
239 int NOld = m_BlobList.GetBlobNum();
240 int NNew = m_BlobListNew.GetBlobNum();
242 for(int i=0; i<NOld; i++)
243 { /* Set 0 collision and clear all hyp: */
244 DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);
246 pF->pBlobHyp->Clear();
247 } /* Set 0 collision. */
249 /* Create correspondence records: */
250 for(int j=0; j<NNew; ++j)
252 CvBlob* pB1 = m_BlobListNew.GetBlob(j);
253 DefBlobTrackerCR* pFLast = NULL;
255 for(int i=0; i<NOld; i++)
256 { /* Check intersection: */
257 int Intersection = 0;
258 DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);
259 CvBlob* pB2 = &(pF->BlobPredict);
261 if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
262 fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Intersection = 1;
268 pF->Collision = pFLast->Collision = 1;
271 pF->pBlobHyp->AddBlob(pB1);
273 } /* Check intersection. */
274 } /* Check next new blob. */
275 } /* Resolve new blob to old. */
277 for(int i=m_BlobList.GetBlobNum(); i>0; --i)
278 { /* Track each blob. */
279 CvBlob* pB = m_BlobList.GetBlob(i-1);
280 DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)pB;
281 int BlobID = CV_BLOB_ID(pB);
282 //CvBlob* pBBest = NULL;
283 //double DistBest = -1;
287 pBT->pResolver->SetCollision(pBT->Collision);
291 { /* Tracking in collision: */
294 pB[0] = pBT->pResolver->Process(&(pBT->BlobPredict),pImg, pImgFG)[0];
296 } /* Tracking in collision. */
298 { /* Non-collision tracking: */
299 CvBlob NewCC = pBT->BlobPredict;
300 if(pBT->pBlobHyp->GetBlobNum()==1)
301 { /* One blob to one CC: */
302 NewCC = pBT->pBlobHyp->GetBlob(0)[0];
305 { /* One blob several CC: */
306 CvBlob* pBBest = NULL;
307 double DistBest = -1;
309 for(int j=pBT->pBlobHyp->GetBlobNum();j>0;--j)
310 { /* Find best CC: */
311 CvBlob* pBNew = pBT->pBlobHyp->GetBlob(j-1);
313 { /* Choose CC by confidence: */
314 // double dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
315 // double dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
316 double C = pBT->pResolver->GetConfidence(pBNew,pImg, pImgFG);
317 if(C > CMax || pBBest == NULL)
324 { /* Choose CC by distance: */
325 double dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
326 double dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
327 double Dist = sqrt(dx*dx+dy*dy);
328 if(Dist < DistBest || pBBest == NULL)
334 } /* Find best CC. */
337 } /* One blob several CC. */
340 pB->w = (m_AlphaSize)*NewCC.w+(1-m_AlphaSize)*pB->w;
341 pB->h = (m_AlphaSize)*NewCC.h+(1-m_AlphaSize)*pB->h;
342 pBT->pResolver->SkipProcess(&(pBT->BlobPredict),pImg, pImgFG);
343 } /* Non-collision tracking. */
345 pBT->pResolver->Update(pB, pImg, pImgFG);
347 CV_BLOB_ID(pB)=BlobID;
349 } /* Track next blob. */
353 IplImage* pI = cvCloneImage(pImg);
354 for(int i=m_BlobListNew.GetBlobNum(); i>0; --i)
355 { /* Draw each new CC: */
356 CvBlob* pB = m_BlobListNew.GetBlob(i-1);
357 CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
358 int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
359 CvSize s = cvSize(MAX(1,x), MAX(1,y));
365 CV_RGB(255,255,0), 1 );
368 for(int i=m_BlobList.GetBlobNum(); i>0; --i)
369 { /* Draw each new CC: */
370 DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
371 CvBlob* pB = &(pF->BlobPredict);
372 CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
373 int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
374 CvSize s = cvSize(MAX(1,x), MAX(1,y));
379 CV_RGB(0,0,255), 1 );
382 p = cvPointFrom32f(CV_BLOB_CENTER(pB));
383 x = cvRound(CV_BLOB_RX(pB)); y = cvRound(CV_BLOB_RY(pB));
384 s = cvSize(MAX(1,x), MAX(1,y));
389 CV_RGB(0,255,0), 1 );
392 //cvNamedWindow("CCwithCR",0);
393 //cvShowImage("CCwithCR",pI);
399 virtual void SaveState(CvFileStorage* fs)
401 int b,bN = m_BlobList.GetBlobNum();
402 cvWriteInt(fs,"BlobNum",m_BlobList.GetBlobNum());
403 cvStartWriteStruct(fs,"BlobList",CV_NODE_SEQ);
407 DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(b);
408 cvStartWriteStruct(fs,NULL,CV_NODE_MAP);
409 cvWriteInt(fs,"ID",CV_BLOB_ID(pF));
410 cvStartWriteStruct(fs,"Blob",CV_NODE_SEQ|CV_NODE_FLOW);
411 cvWriteRawData(fs,&(pF->blob),1,"ffffi");
412 cvEndWriteStruct(fs);
413 cvStartWriteStruct(fs,"BlobPredict",CV_NODE_SEQ|CV_NODE_FLOW);
414 cvWriteRawData(fs,&(pF->BlobPredict),1,"ffffi");
415 cvEndWriteStruct(fs);
416 cvStartWriteStruct(fs,"BlobPrev",CV_NODE_SEQ|CV_NODE_FLOW);
417 cvWriteRawData(fs,&(pF->BlobPrev),1,"ffffi");
418 cvEndWriteStruct(fs);
419 pF->pBlobHyp->Write(fs,"BlobHyp");
420 cvWriteInt(fs,"Collision",pF->Collision);
422 cvStartWriteStruct(fs,"Predictor",CV_NODE_MAP);
423 pF->pPredictor->SaveState(fs);
424 cvEndWriteStruct(fs);
426 cvStartWriteStruct(fs,"Resolver",CV_NODE_MAP);
427 pF->pResolver->SaveState(fs);
428 cvEndWriteStruct(fs);
429 cvEndWriteStruct(fs);
432 cvEndWriteStruct(fs);
436 virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
438 int b,bN = cvReadIntByName(fs,node,"BlobNum",0);
439 CvFileNode* pBlobListNode = cvGetFileNodeByName(fs,node,"BlobList");
440 if(!CV_NODE_IS_SEQ(pBlobListNode->tag)) return;
441 bN = pBlobListNode->data.seq->total;
445 DefBlobTrackerCR* pF = NULL;
447 CvFileNode* pSeqNode = NULL;
448 CvFileNode* pBlobNode = (CvFileNode*)cvGetSeqElem(pBlobListNode->data.seq,b);
451 Blob.ID = cvReadIntByName(fs,pBlobNode,"ID",0);
453 pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Blob");
454 if(CV_NODE_IS_SEQ(pSeqNode->tag))
455 cvReadRawData( fs, pSeqNode, &Blob, "ffffi" );
457 AddBlob(&Blob,NULL,NULL);
458 pF = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(Blob.ID);
460 pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPredict");
461 if(CV_NODE_IS_SEQ(pSeqNode->tag))
462 cvReadRawData( fs, pSeqNode, &pF->BlobPredict, "ffffi" );
464 pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPrev");
465 if(CV_NODE_IS_SEQ(pSeqNode->tag))
466 cvReadRawData( fs, pSeqNode, &pF->BlobPrev, "ffffi" );
468 pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobHyp");
470 pF->pBlobHyp->Load(fs,pSeqNode);
471 pF->Collision = cvReadIntByName(fs, pBlobNode,"Collision",pF->Collision);
473 pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Predictor");
475 pF->pPredictor->LoadState(fs,pSeqNode);
477 pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Resolver");
479 pF->pResolver->LoadState(fs,pSeqNode);
480 } /* Read next blob. */
481 } /* CCwithCR LoadState */
483 //void SetCollision(int Collision){m_Collision = Collision;}
486 CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF();
487 CvBlobTracker* cvCreateBlobTrackerCCMSPF()
489 return (CvBlobTracker*) new CvBlobTrackerCCCR(cvCreateBlobTrackerOneMSPF,"MSPF");
491 /*============== BLOB TRACKERCC CLASS DECLARATION =============== */