mirror of
https://github.com/opencv/opencv.git
synced 2025-01-09 21:27:59 +08:00
492 lines
18 KiB
C++
492 lines
18 KiB
C++
/*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 "precomp.hpp"
|
|
|
|
/* 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;
|
|
//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(int 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 NOld = m_BlobList.GetBlobNum();
|
|
int NNew = m_BlobListNew.GetBlobNum();
|
|
|
|
for(int 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(int j=0; j<NNew; ++j)
|
|
{
|
|
CvBlob* pB1 = m_BlobListNew.GetBlob(j);
|
|
DefBlobTrackerCR* pFLast = NULL;
|
|
|
|
for(int 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(int 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;
|
|
|
|
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(int 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);
|
|
for(int 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(int 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 =============== */
|