--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+// By downloading, copying, installing or using the software you agree to this license.
+// If you do not agree to this license, do not download, install,
+// copy or use the software.
+//
+//
+// Intel License Agreement
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+// * Redistribution's of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+//
+// * Redistribution's in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+//
+// * The name of Intel Corporation may not be used to endorse or promote products
+// derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "_cvaux.h"
+
+/* Blob (foreground-pixel connected-component) tracking with collision resolution.
+ *
+ * For entrypoints into the literature see:
+ *
+ * A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking
+ * Arulampalam &t al, 2001, 15p
+ * http://www-clmc.usc.edu/publications/A/arulampalam-TSP2002.pdf
+ *
+ * Particle Filters for Positioning, Navigation, and Tracking
+ * Gustafsson et al, 2002 12p
+ * http://www.control.isy.liu.se/~fredrik/reports/01SPpf4pos.pdf
+ *
+ * Particle Filtering in High Clutter Environments
+ * Korhonen et al, 2005 4p
+ * http://www.cs.uku.fi/finsig05/papers/paper26_FINSIG05.pdf
+ *
+ * Appearance Models for Occlusion Handling
+ * Andrew Senior &t al, 8p 2001
+ * http://www.research.ibm.com/peoplevision/PETS2001.pdf
+ *
+ */
+
+/*============== BLOB TRACKERCC CLASS DECLARATION =============== */
+typedef struct DefBlobTrackerCR
+{
+ CvBlob blob;
+ CvBlobTrackPredictor* pPredictor;
+ CvBlob BlobPredict;
+ CvBlob BlobPrev;
+ int Collision;
+ CvBlobSeq* pBlobHyp;
+ CvBlobTrackerOne* pResolver;
+} DefBlobTrackerCR;
+
+void cvFindBlobsByCCClasters(IplImage* pFG, CvBlobSeq* pBlobs, CvMemStorage* storage);
+
+class CvBlobTrackerCCCR : public CvBlobTracker
+{
+private:
+ float m_AlphaSize;
+ int m_Collision;
+ CvBlobSeq m_BlobList;
+ CvBlobSeq m_BlobListNew;
+ CvMemStorage* m_pMem;
+ CvBlobTrackerOne* (*m_CreateCR)();
+ char m_ModuleName[1024];
+
+
+public:
+ CvBlobTrackerCCCR(CvBlobTrackerOne* (*CreateCR)(), const char* CRName):m_BlobList(sizeof(DefBlobTrackerCR))
+ {
+ m_CreateCR = CreateCR;
+ m_pMem = cvCreateMemStorage();
+
+ m_Collision = 1; /* if 1 then collistion will be detected and processed */
+
+ m_AlphaSize = 0.05f;
+ AddParam("AlphaSize",&m_AlphaSize);
+ CommentParam("AlphaSize", "Size update speed (0..1)");
+
+ strcpy(m_ModuleName, "CCCR");
+ if(CRName)strcat(m_ModuleName,CRName);
+ SetModuleName(m_ModuleName);
+
+ {
+ CvBlobTrackerOne* pM = m_CreateCR();
+ TransferParamsFromChild(pM,NULL);
+ pM->Release();
+ }
+ SetParam("SizeVar",0);
+ };
+
+ ~CvBlobTrackerCCCR()
+ {
+ if(m_pMem)cvReleaseMemStorage(&m_pMem);
+ };
+
+ /* Blob functions: */
+ virtual int GetBlobNum() {return m_BlobList.GetBlobNum();};
+ virtual CvBlob* GetBlob(int BlobIndex){return m_BlobList.GetBlob(BlobIndex);};
+ virtual void SetBlob(int BlobIndex, CvBlob* pBlob)
+ {
+ CvBlob* pB = m_BlobList.GetBlob(BlobIndex);
+ if(pB) pB[0] = pBlob[0];
+ };
+
+ virtual CvBlob* GetBlobByID(int BlobID){return m_BlobList.GetBlobByID(BlobID);};
+ virtual void DelBlob(int BlobIndex)
+ {
+ DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(BlobIndex);
+ if(pBT->pResolver)pBT->pResolver->Release();
+ if(pBT->pPredictor)pBT->pPredictor->Release();
+ delete pBT->pBlobHyp;
+ m_BlobList.DelBlob(BlobIndex);
+ };
+
+ virtual void DelBlobByID(int BlobID)
+ {
+ DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(BlobID);
+ if(pBT->pResolver)pBT->pResolver->Release();
+ if(pBT->pPredictor)pBT->pPredictor->Release();
+ delete pBT->pBlobHyp;
+ m_BlobList.DelBlobByID(BlobID);
+ };
+
+ virtual void Release(){delete this;};
+
+ /* Add new blob to track it and assign to this blob personal ID */
+ /* pBlob - pinter to structure with blob parameters (ID is ignored)*/
+ /* pImg - current image */
+ /* pImgFG - current foreground mask */
+ /* Return pointer to new added blob: */
+ virtual CvBlob* AddBlob(CvBlob* pB, IplImage* pImg, IplImage* pImgFG = NULL )
+ {
+ DefBlobTrackerCR NewB;
+ NewB.blob = pB[0];
+ NewB.pBlobHyp = new CvBlobSeq;
+ NewB.pPredictor = cvCreateModuleBlobTrackPredictKalman(); /* module for predict position */
+ NewB.pPredictor->SetParam("DataNoisePos",0.001);
+ NewB.pPredictor->ParamUpdate();
+ NewB.pResolver = NULL;
+ if(m_CreateCR)
+ {
+ NewB.pResolver = m_CreateCR();
+ TransferParamsToChild(NewB.pResolver,NULL);
+ NewB.pResolver->Init(pB, pImg, pImgFG);
+ }
+ m_BlobList.AddBlob((CvBlob*)&NewB);
+ return m_BlobList.GetBlob(m_BlobList.GetBlobNum()-1);
+ };
+
+ virtual void Process(IplImage* pImg, IplImage* pImgFG = NULL)
+ {
+ CvSeq* cnts;
+ CvSeq* cnt;
+ int i;
+ //CvMat* pMC = NULL;
+
+ if(m_BlobList.GetBlobNum() <= 0 ) return;
+
+ /* Clear blob list for new blobs: */
+ m_BlobListNew.Clear();
+
+ assert(m_pMem);
+ cvClearMemStorage(m_pMem);
+ assert(pImgFG);
+
+ { /* One contour - one blob: */
+ IplImage* pBin = cvCloneImage(pImgFG);
+ assert(pBin);
+ cvThreshold(pBin,pBin,128,255,CV_THRESH_BINARY);
+ cvFindContours(pBin, m_pMem, &cnts, sizeof(CvContour), CV_RETR_EXTERNAL);
+
+ /* Process each contour: */
+ for(cnt = cnts; cnt; cnt=cnt->h_next)
+ {
+ CvBlob NewBlob;
+
+ /* Image moments: */
+ double M00,X,Y,XX,YY;
+ CvMoments m;
+ CvRect r = ((CvContour*)cnt)->rect;
+ CvMat mat;
+ if(r.height < 3 || r.width < 3) continue;
+ cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
+ M00 = cvGetSpatialMoment( &m, 0, 0 );
+ if(M00 <= 0 ) continue;
+ X = cvGetSpatialMoment( &m, 1, 0 )/M00;
+ Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
+ XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
+ YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
+ NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));
+ m_BlobListNew.AddBlob(&NewBlob);
+
+ } /* Next contour. */
+
+ cvReleaseImage(&pBin);
+ }
+
+ for(i=m_BlobList.GetBlobNum(); i>0; --i)
+ { /* Predict new blob position. */
+ CvBlob* pB = NULL;
+ DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
+
+ /* Update predictor. */
+ pBT->pPredictor->Update(&(pBT->blob));
+ pB = pBT->pPredictor->Predict();
+ if(pB)
+ {
+ pBT->BlobPredict = pB[0];
+ }
+ pBT->BlobPrev = pBT->blob;
+ } /* Predict new blob position. */
+
+
+ if(m_BlobList.GetBlobNum()>0 && m_BlobListNew.GetBlobNum()>0)
+ { /* Resolve new blob to old: */
+ int i,j;
+ int NOld = m_BlobList.GetBlobNum();
+ int NNew = m_BlobListNew.GetBlobNum();
+
+ for(i=0; i<NOld; i++)
+ { /* Set 0 collision and clear all hyp: */
+ DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);
+ pF->Collision = 0;
+ pF->pBlobHyp->Clear();
+ } /* Set 0 collision. */
+
+ /* Create correspondence records: */
+ for(j=0; j<NNew; ++j)
+ {
+ CvBlob* pB1 = m_BlobListNew.GetBlob(j);
+ DefBlobTrackerCR* pFLast = NULL;
+
+ for(i=0; i<NOld; i++)
+ { /* Check intersection: */
+ int Intersection = 0;
+ DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i);
+ CvBlob* pB2 = &(pF->BlobPredict);
+
+ if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
+ fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Intersection = 1;
+
+ if(Intersection)
+ {
+ if(pFLast)
+ {
+ pF->Collision = pFLast->Collision = 1;
+ }
+ pFLast = pF;
+ pF->pBlobHyp->AddBlob(pB1);
+ }
+ } /* Check intersection. */
+ } /* Check next new blob. */
+ } /* Resolve new blob to old. */
+
+ for(i=m_BlobList.GetBlobNum(); i>0; --i)
+ { /* Track each blob. */
+ CvBlob* pB = m_BlobList.GetBlob(i-1);
+ DefBlobTrackerCR* pBT = (DefBlobTrackerCR*)pB;
+ int BlobID = CV_BLOB_ID(pB);
+ //CvBlob* pBBest = NULL;
+ //double DistBest = -1;
+ int j;
+
+ if(pBT->pResolver)
+ {
+ pBT->pResolver->SetCollision(pBT->Collision);
+ }
+
+ if(pBT->Collision)
+ { /* Tracking in collision: */
+ if(pBT->pResolver)
+ {
+ pB[0] = pBT->pResolver->Process(&(pBT->BlobPredict),pImg, pImgFG)[0];
+ }
+ } /* Tracking in collision. */
+ else
+ { /* Non-collision tracking: */
+ CvBlob NewCC = pBT->BlobPredict;
+ if(pBT->pBlobHyp->GetBlobNum()==1)
+ { /* One blob to one CC: */
+ NewCC = pBT->pBlobHyp->GetBlob(0)[0];
+ }
+ else
+ { /* One blob several CC: */
+ CvBlob* pBBest = NULL;
+ double DistBest = -1;
+ double CMax = 0;
+ for(j=pBT->pBlobHyp->GetBlobNum();j>0;--j)
+ { /* Find best CC: */
+ CvBlob* pBNew = pBT->pBlobHyp->GetBlob(j-1);
+ if(pBT->pResolver)
+ { /* Choose CC by confidence: */
+// double dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
+// double dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
+ double C = pBT->pResolver->GetConfidence(pBNew,pImg, pImgFG);
+ if(C > CMax || pBBest == NULL)
+ {
+ CMax = C;
+ pBBest = pBNew;
+ }
+ }
+ else
+ { /* Choose CC by distance: */
+ double dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
+ double dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
+ double Dist = sqrt(dx*dx+dy*dy);
+ if(Dist < DistBest || pBBest == NULL)
+ {
+ DistBest = Dist;
+ pBBest = pBNew;
+ }
+ }
+ } /* Find best CC. */
+ if(pBBest)
+ NewCC = pBBest[0];
+ } /* One blob several CC. */
+ pB->x = NewCC.x;
+ pB->y = NewCC.y;
+ pB->w = (m_AlphaSize)*NewCC.w+(1-m_AlphaSize)*pB->w;
+ pB->h = (m_AlphaSize)*NewCC.h+(1-m_AlphaSize)*pB->h;
+ pBT->pResolver->SkipProcess(&(pBT->BlobPredict),pImg, pImgFG);
+ } /* Non-collision tracking. */
+
+ pBT->pResolver->Update(pB, pImg, pImgFG);
+
+ CV_BLOB_ID(pB)=BlobID;
+
+ } /* Track next blob. */
+
+ if(m_Wnd)
+ {
+ IplImage* pI = cvCloneImage(pImg);
+ int i;
+ for(i=m_BlobListNew.GetBlobNum(); i>0; --i)
+ { /* Draw each new CC: */
+ CvBlob* pB = m_BlobListNew.GetBlob(i-1);
+ CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
+ int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
+ CvSize s = cvSize(MAX(1,x), MAX(1,y));
+ //int c = 255;
+ cvEllipse( pI,
+ p,
+ s,
+ 0, 0, 360,
+ CV_RGB(255,255,0), 1 );
+ }
+
+ for(i=m_BlobList.GetBlobNum(); i>0; --i)
+ { /* Draw each new CC: */
+ DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(i-1);
+ CvBlob* pB = &(pF->BlobPredict);
+ CvPoint p = cvPointFrom32f(CV_BLOB_CENTER(pB));
+ int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
+ CvSize s = cvSize(MAX(1,x), MAX(1,y));
+ cvEllipse( pI,
+ p,
+ s,
+ 0, 0, 360,
+ CV_RGB(0,0,255), 1 );
+
+ pB = &(pF->blob);
+ p = cvPointFrom32f(CV_BLOB_CENTER(pB));
+ x = cvRound(CV_BLOB_RX(pB)); y = cvRound(CV_BLOB_RY(pB));
+ s = cvSize(MAX(1,x), MAX(1,y));
+ cvEllipse( pI,
+ p,
+ s,
+ 0, 0, 360,
+ CV_RGB(0,255,0), 1 );
+ }
+
+ //cvNamedWindow("CCwithCR",0);
+ //cvShowImage("CCwithCR",pI);
+ cvReleaseImage(&pI);
+ }
+
+ } /* Process. */
+
+ virtual void SaveState(CvFileStorage* fs)
+ {
+ int b,bN = m_BlobList.GetBlobNum();
+ cvWriteInt(fs,"BlobNum",m_BlobList.GetBlobNum());
+ cvStartWriteStruct(fs,"BlobList",CV_NODE_SEQ);
+
+ for(b=0; b<bN; ++b)
+ {
+ DefBlobTrackerCR* pF = (DefBlobTrackerCR*)m_BlobList.GetBlob(b);
+ cvStartWriteStruct(fs,NULL,CV_NODE_MAP);
+ cvWriteInt(fs,"ID",CV_BLOB_ID(pF));
+ cvStartWriteStruct(fs,"Blob",CV_NODE_SEQ|CV_NODE_FLOW);
+ cvWriteRawData(fs,&(pF->blob),1,"ffffi");
+ cvEndWriteStruct(fs);
+ cvStartWriteStruct(fs,"BlobPredict",CV_NODE_SEQ|CV_NODE_FLOW);
+ cvWriteRawData(fs,&(pF->BlobPredict),1,"ffffi");
+ cvEndWriteStruct(fs);
+ cvStartWriteStruct(fs,"BlobPrev",CV_NODE_SEQ|CV_NODE_FLOW);
+ cvWriteRawData(fs,&(pF->BlobPrev),1,"ffffi");
+ cvEndWriteStruct(fs);
+ pF->pBlobHyp->Write(fs,"BlobHyp");
+ cvWriteInt(fs,"Collision",pF->Collision);
+
+ cvStartWriteStruct(fs,"Predictor",CV_NODE_MAP);
+ pF->pPredictor->SaveState(fs);
+ cvEndWriteStruct(fs);
+
+ cvStartWriteStruct(fs,"Resolver",CV_NODE_MAP);
+ pF->pResolver->SaveState(fs);
+ cvEndWriteStruct(fs);
+ cvEndWriteStruct(fs);
+ }
+
+ cvEndWriteStruct(fs);
+
+ } /* SaveState. */
+
+ virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
+ {
+ int b,bN = cvReadIntByName(fs,node,"BlobNum",0);
+ CvFileNode* pBlobListNode = cvGetFileNodeByName(fs,node,"BlobList");
+ if(!CV_NODE_IS_SEQ(pBlobListNode->tag)) return;
+ bN = pBlobListNode->data.seq->total;
+
+ for(b=0; b<bN; ++b)
+ {
+ DefBlobTrackerCR* pF = NULL;
+ CvBlob Blob;
+ CvFileNode* pSeqNode = NULL;
+ CvFileNode* pBlobNode = (CvFileNode*)cvGetSeqElem(pBlobListNode->data.seq,b);
+ assert(pBlobNode);
+
+ Blob.ID = cvReadIntByName(fs,pBlobNode,"ID",0);
+
+ pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Blob");
+ if(CV_NODE_IS_SEQ(pSeqNode->tag))
+ cvReadRawData( fs, pSeqNode, &Blob, "ffffi" );
+
+ AddBlob(&Blob,NULL,NULL);
+ pF = (DefBlobTrackerCR*)m_BlobList.GetBlobByID(Blob.ID);
+
+ pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPredict");
+ if(CV_NODE_IS_SEQ(pSeqNode->tag))
+ cvReadRawData( fs, pSeqNode, &pF->BlobPredict, "ffffi" );
+
+ pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobPrev");
+ if(CV_NODE_IS_SEQ(pSeqNode->tag))
+ cvReadRawData( fs, pSeqNode, &pF->BlobPrev, "ffffi" );
+
+ pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "BlobHyp");
+ if(pSeqNode)
+ pF->pBlobHyp->Load(fs,pSeqNode);
+ pF->Collision = cvReadIntByName(fs, pBlobNode,"Collision",pF->Collision);
+
+ pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Predictor");
+ if(pSeqNode)
+ pF->pPredictor->LoadState(fs,pSeqNode);
+
+ pSeqNode = cvGetFileNodeByName(fs, pBlobNode, "Resolver");
+ if(pSeqNode)
+ pF->pResolver->LoadState(fs,pSeqNode);
+ } /* Read next blob. */
+ } /* CCwithCR LoadState */
+
+ //void SetCollision(int Collision){m_Collision = Collision;};
+};
+
+CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF();
+CvBlobTracker* cvCreateBlobTrackerCCMSPF()
+{
+ return (CvBlobTracker*) new CvBlobTrackerCCCR(cvCreateBlobTrackerOneMSPF,"MSPF");
+}
+/*============== BLOB TRACKERCC CLASS DECLARATION =============== */
projects / opencv / blobdiff