/*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"
static float CalcAverageMask(CvBlob* pBlob, IplImage* pImgFG )
{ /* Calculate sum of mask: */
double Area, Aver = 0;
CvRect r;
CvMat mat;
if(pImgFG==NULL) return 0;
r.x = cvRound(pBlob->x - pBlob->w*0.5);
r.y = cvRound(pBlob->y - pBlob->h*0.5);
r.width = cvRound(pBlob->w);
r.height = cvRound(pBlob->h);
Area = r.width*r.height;
if(r.x<0){r.width += r.x;r.x = 0;}
if(r.y<0){r.height += r.y;r.y = 0;}
if((r.x+r.width)>=pImgFG->width){r.width=pImgFG->width-r.x-1;}
if((r.y+r.height)>=pImgFG->height){r.height=pImgFG->height-r.y-1;}
if(r.width>0 && r.height>0)
{
double Sum = cvSum(cvGetSubRect(pImgFG,&mat,r)).val[0]/255.0;
assert(Area>0);
Aver = Sum/Area;
}
return (float)Aver;
} /* Calculate sum of mask. */
/*============== BLOB TRACKERCC CLASS DECLARATION =============== */
typedef struct DefBlobTracker
{
CvBlob blob;
CvBlobTrackPredictor* pPredictor;
CvBlob BlobPredict;
int Collision;
CvBlobSeq* pBlobHyp;
float AverFG;
} DefBlobTracker;
void cvFindBlobsByCCClasters(IplImage* pFG, CvBlobSeq* pBlobs, CvMemStorage* storage);
class CvBlobTrackerCC : public CvBlobTracker
{
private:
float m_AlphaSize;
float m_AlphaPos;
float m_Alpha;
int m_Collision;
int m_ConfidenceType;
const char* m_ConfidenceTypeStr;
CvBlobSeq m_BlobList;
CvBlobSeq m_BlobListNew;
// int m_LastID;
CvMemStorage* m_pMem;
int m_ClearHyp;
IplImage* m_pImg;
IplImage* m_pImgFG;
public:
CvBlobTrackerCC():m_BlobList(sizeof(DefBlobTracker))
{
// m_LastID = 0;
m_ClearHyp = 0;
m_pMem = cvCreateMemStorage();
m_Collision = 1; /* if 1 then collistion will be detected and processed */
AddParam("Collision",&m_Collision);
CommentParam("Collision", "If 1 then collision cases are processed in special way");
m_AlphaSize = 0.02f;
AddParam("AlphaSize",&m_AlphaSize);
CommentParam("AlphaSize", "Size update speed (0..1)");
m_AlphaPos = 1.0f;
AddParam("AlphaPos",&m_AlphaPos);
CommentParam("AlphaPos", "Position update speed (0..1)");
m_Alpha = 0.001f;
AddParam("Alpha", &m_Alpha);
CommentParam("Alpha","Coefficient for model histogram updating (0 - hist is not updated)");
m_ConfidenceType=0;
m_ConfidenceTypeStr = "NearestBlob";
AddParam("ConfidenceType", &m_ConfidenceTypeStr);
CommentParam("ConfidenceType","Type of calculated Confidence (NearestBlob, AverFG, BC)");
SetModuleName("CC");
}
~CvBlobTrackerCC()
{
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)
{
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
if(pBT==NULL) return;
if(pBT->pPredictor)
{
pBT->pPredictor->Release();
}
else
{
printf("WARNING!!! Invalid Predictor in CC tracker");
}
delete pBT->pBlobHyp;
m_BlobList.DelBlob(BlobIndex);
}
#if 0
virtual void DelBlobByID(int BlobID)
{
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlobByID(BlobID);
pBT->pPredictor->Release();
delete pBT->pBlobHyp;
m_BlobList.DelBlobByID(BlobID);
};
#endif
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 )
{
assert(pImgFG); /* This tracker uses only foreground mask. */
DefBlobTracker NewB;
NewB.blob = pB[0];
// CV_BLOB_ID(&NewB) = m_LastID;
NewB.pBlobHyp = new CvBlobSeq;
NewB.pPredictor = cvCreateModuleBlobTrackPredictKalman(); /* Module for position prediction. */
NewB.pPredictor->Update(pB);
NewB.AverFG = pImgFG?CalcAverageMask(pB,pImgFG):0;
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;
m_pImg = pImg;
m_pImgFG = pImgFG;
if(m_BlobList.GetBlobNum() <= 0 ) return;
/* Clear bloblist for new blobs: */
m_BlobListNew.Clear();
assert(m_pMem);
cvClearMemStorage(m_pMem);
assert(pImgFG);
/* Find CC: */
#if 0
{ // By contour clustering:
cvFindBlobsByCCClasters(pImgFG, &m_BlobListNew, m_pMem);
}
#else
{ /* 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);
}
#endif
for(i=m_BlobList.GetBlobNum(); i>0; --i)
{ /* Predict new blob position: */
CvBlob* pB=NULL;
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(i-1);
/* Update predictor by previous value of blob: */
pBT->pPredictor->Update(&(pBT->blob));
/* Predict current position: */
pB = pBT->pPredictor->Predict();
if(pB)
{
pBT->BlobPredict = pB[0];
}
else
{
pBT->BlobPredict = pBT->blob;
}
} /* Predict new blob position. */
if(m_Collision)
for(i=m_BlobList.GetBlobNum(); i>0; --i)
{ /* Predict collision. */
int Collision = 0;
int j;
DefBlobTracker* pF = (DefBlobTracker*)m_BlobList.GetBlob(i-1);
for(j=m_BlobList.GetBlobNum(); j>0; --j)
{ /* Predict collision: */
CvBlob* pB1;
CvBlob* pB2;
DefBlobTracker* pF2 = (DefBlobTracker*)m_BlobList.GetBlob(j-1);
if(i==j) continue;
pB1 = &pF->BlobPredict;
pB2 = &pF2->BlobPredict;
if( fabs(pB1->x-pB2->x)<0.6*(pB1->w+pB2->w) &&
fabs(pB1->y-pB2->y)<0.6*(pB1->h+pB2->h) ) Collision = 1;
pB1 = &pF->blob;
pB2 = &pF2->blob;
if( fabs(pB1->x-pB2->x)<0.6*(pB1->w+pB2->w) &&
fabs(pB1->y-pB2->y)<0.6*(pB1->h+pB2->h) ) Collision = 1;
if(Collision) break;
} /* Check next blob to cross current. */
pF->Collision = Collision;
} /* Predict collision. */
for(i=m_BlobList.GetBlobNum(); i>0; --i)
{ /* Find a neighbour on current frame
* for each blob from previous frame:
*/
CvBlob* pBl = m_BlobList.GetBlob(i-1);
DefBlobTracker* pBT = (DefBlobTracker*)pBl;
//int BlobID = CV_BLOB_ID(pB);
//CvBlob* pBBest = NULL;
//double DistBest = -1;
//int j;
if(pBT->pBlobHyp->GetBlobNum()>0)
{ /* Track all hypotheses: */
int h,hN = pBT->pBlobHyp->GetBlobNum();
for(h=0; h<hN; ++h)
{
int j, jN = m_BlobListNew.GetBlobNum();
CvBlob* pB = pBT->pBlobHyp->GetBlob(h);
int BlobID = CV_BLOB_ID(pB);
CvBlob* pBBest = NULL;
double DistBest = -1;
for(j=0; j<jN; j++)
{ /* Find best CC: */
double Dist = -1;
CvBlob* pBNew = m_BlobListNew.GetBlob(j);
double dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
double dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
if(dx > 2*CV_BLOB_WX(pB) || dy > 2*CV_BLOB_WY(pB)) continue;
Dist = sqrt(dx*dx+dy*dy);
if(Dist < DistBest || pBBest == NULL)
{
DistBest = Dist;
pBBest = pBNew;
}
} /* Find best CC. */
if(pBBest)
{
pB[0] = pBBest[0];
CV_BLOB_ID(pB) = BlobID;
}
else
{ /* Delete this hypothesis. */
pBT->pBlobHyp->DelBlob(h);
h--;
hN--;
}
} /* Next hypothysis. */
} /* Track all hypotheses. */
} /* Track next blob. */
m_ClearHyp = 1;
} /* Process. */
virtual void ProcessBlob(int BlobIndex, CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
{
int ID = pBlob->ID;
CvBlob* pB = m_BlobList.GetBlob(BlobIndex);
DefBlobTracker* pBT = (DefBlobTracker*)pB;
//CvBlob* pBBest = NULL;
//double DistBest = -1;
int BlobID;
if(pB==NULL) return;
BlobID = pB->ID;
if(m_Collision && pBT->Collision)
{ /* Tracking in collision: */
pB[0]=pBT->BlobPredict;
CV_BLOB_ID(pB)=BlobID;
} /* Tracking in collision. */
else
{ /* Non-collision tracking: */
CvBlob* pBBest = GetNearestBlob(pB);
if(pBBest)
{
float w = pBlob->w*(1-m_AlphaSize)+m_AlphaSize*pBBest->w;
float h = pBlob->h*(1-m_AlphaSize)+m_AlphaSize*pBBest->h;
float x = pBlob->x*(1-m_AlphaPos)+m_AlphaPos*pBBest->x;
float y = pBlob->y*(1-m_AlphaPos)+m_AlphaPos*pBBest->y;
pB->w = w;
pB->h = h;
pB->x = x;
pB->y = y;
CV_BLOB_ID(pB) = BlobID;
}
} /* Non-collision tracking. */
pBlob[0] = pB[0];
pBlob->ID = ID;
}
virtual double GetConfidence(int BlobIndex, CvBlob* pBlob, IplImage* /*pImg*/, IplImage* pImgFG = NULL)
{
/* Define coefficients in exp by exp(-XT*K)=VT: */
static double _KS = -log(0.1)/pow(0.5,2); /* XT = 1, VT = 0.1 - when size is Larger in 2 times Confidence is smoller in 10 times */
static double _KP = -log(0.1)/pow(m_pImg->width*0.02,2); /* XT = 0.02*ImgWidth, VT = 0.1*/
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
float dx,dy,dw,dh;
float dp2,ds2;
double W = 1;
CvBlob* pBC = GetNearestBlob(pBlob);
if(pBC == NULL ) return 0;
dx = pBC->x-pBlob->x;
dy = pBC->y-pBlob->y;
dw = (pBC->w-pBlob->w)/pBC->w;
dh = (pBC->h-pBlob->h)/pBC->h;
dp2 = dx*dx+dy*dy;
ds2 = dw*dw+dh*dh;
if(!pBT->Collision)
{ /* Confidence for size by nearest blob: */
W*=exp(-_KS*ds2);
}
if(m_ConfidenceType==0 && !pBT->Collision)
{ /* Confidence by nearest blob: */
W*=exp(-_KP*dp2);
}
if(m_ConfidenceType==1 && pBT->AverFG>0)
{ /* Calculate sum of mask: */
float Aver = CalcAverageMask(pBlob, pImgFG );
if(Aver < pBT->AverFG)
{
float diff = 1+0.9f*(Aver-pBT->AverFG)/pBT->AverFG;
if(diff < 0.1f) diff = 0.1f;
W *= diff;
}
} /* Calculate sum of mask. */
if(m_ConfidenceType==2)
{ /* Calculate BCoeff: */
float S = 0.2f;
float Aver = CalcAverageMask(pBlob, pImgFG );
double B = sqrt(Aver*pBT->AverFG)+sqrt((1-Aver)*(1-pBT->AverFG));
W *= exp((B-1)/(2*S));
} /* Calculate sum of mask. */
return W;
}
virtual void UpdateBlob(int BlobIndex, CvBlob* /*pBlob*/, IplImage* /*pImg*/, IplImage* pImgFG = NULL)
{
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
if(pImgFG==NULL || pBT==NULL) return;
if(!pBT->Collision)
{
//pBT->AverFG = pBT->AverFG * (1-m_Alpha) + m_Alpha * CalcAverageMask(pBlob,pImgFG);
}
}
virtual void ParamUpdate()
{
const char* pCT[3] = {"NearestBlob","AverFG","BC"};
int i;
CvBlobTracker::ParamUpdate();
for(i=0; i<3; ++i)
{
if(cv_stricmp(m_ConfidenceTypeStr,pCT[i])==0)
{
m_ConfidenceType = i;
}
}
SetParamStr("ConfidenceType",pCT[m_ConfidenceType]);
}
/* =============== MULTI HYPOTHESIS INTERFACE ================== */
/* Return number of position hypotheses of currently tracked blob: */
virtual int GetBlobHypNum(int BlobIdx)
{
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIdx);
assert(pBT->pBlobHyp);
return pBT->pBlobHyp->GetBlobNum();
} /* CvBlobtrackerList::GetBlobHypNum() */
/* Return pointer to specified blob hypothesis by index blob: */
virtual CvBlob* GetBlobHyp(int BlobIndex, int hypothesis)
{
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
assert(pBT->pBlobHyp);
return pBT->pBlobHyp->GetBlob(hypothesis);
} /* CvBlobtrackerList::GetBlobHyp() */
/* Set new parameters for specified (by index) blob hypothesis
* (can be called several times for each hypothesis):
*/
virtual void SetBlobHyp(int BlobIndex, CvBlob* pBlob)
{
if(m_ClearHyp)
{ /* Clear all hypotheses: */
int b, bN = m_BlobList.GetBlobNum();
for(b=0; b<bN; ++b)
{
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(b);
assert(pBT->pBlobHyp);
pBT->pBlobHyp->Clear();
}
m_ClearHyp = 0;
}
{ /* Add hypothesis: */
DefBlobTracker* pBT = (DefBlobTracker*)m_BlobList.GetBlob(BlobIndex);
assert(pBT->pBlobHyp);
pBT->pBlobHyp->AddBlob(pBlob);
}
}
private:
CvBlob* GetNearestBlob(CvBlob* pB)
{
//DefBlobTracker* pBT = (DefBlobTracker*)pB;
CvBlob* pBBest = NULL;
double DistBest = -1;
if(pB==NULL) return NULL;
for(int j=m_BlobListNew.GetBlobNum(); j>0; --j)
{ /* Find best CC: */
double Dist = -1;
CvBlob* pBNew = m_BlobListNew.GetBlob(j-1);
double dx = fabs(CV_BLOB_X(pB)-CV_BLOB_X(pBNew));
double dy = fabs(CV_BLOB_Y(pB)-CV_BLOB_Y(pBNew));
if(dx > 2*CV_BLOB_WX(pB) || dy > 2*CV_BLOB_WY(pB)) continue;
Dist = sqrt(dx*dx+dy*dy);
if(Dist < DistBest || pBBest == NULL)
{
DistBest = Dist;
pBBest = pBNew;
}
} /* Find best CC. */
return pBBest;
} /* GetNearestBlob */
};
CvBlobTracker* cvCreateBlobTrackerCC()
{
return (CvBlobTracker*) new CvBlobTrackerCC;
}
/*============== BLOB TRACKERCC CLASS DECLARATION =============== */