double Vol, WM, WC;
Vol = 0.5*(m_HistVolume + pH->m_HistVolume);
WM = Vol*(1-W)/m_HistVolume;
WC = Vol*(W)/pH->m_HistVolume;
cvAddWeighted(m_pHist, WM, pH->m_pHist,WC,0,m_pHist);
m_HistVolume = (float)cvSum(m_pHist).val[0];
double Vol, WM, WC;
Vol = 0.5*(m_HistVolume + pH->m_HistVolume);
WM = Vol*(1-W)/m_HistVolume;
WC = Vol*(W)/pH->m_HistVolume;
cvAddWeighted(m_pHist, WM, pH->m_pHist,WC,0,m_pHist);
m_HistVolume = (float)cvSum(m_pHist).val[0];
if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
m_KernelHist = cvCreateMat(h, w, DefHistTypeMat);
m_KernelMeanShift = cvCreateMat(h, w, DefHistTypeMat);
if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
m_KernelHist = cvCreateMat(h, w, DefHistTypeMat);
m_KernelMeanShift = cvCreateMat(h, w, DefHistTypeMat);
{
double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
// double r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((y0*y0)+(x0*x0));
{
double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
// double r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((y0*y0)+(x0*x0));
CV_MAT_ELEM(m_KernelMeanShift[0],DefHistType, y, x) = (DefHistType)GetKernelMeanShift(r2);
}
}
CV_MAT_ELEM(m_KernelMeanShift[0],DefHistType, y, x) = (DefHistType)GetKernelMeanShift(r2);
}
}
// void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pHist, DefHistType* pHistVolume)
// void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, DefHist* pHist)
// void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pHist, DefHistType* pHistVolume)
// void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, DefHist* pHist)
void CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
{
int UsePrecalculatedKernel = 0;
void CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
{
int UsePrecalculatedKernel = 0;
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
DefHistType* pKernelData = NULL;
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
DefHistType* pKernelData = NULL;
if(UsePrecalculatedKernel)
{
pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
}
if(UsePrecalculatedKernel)
{
pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
}
double calcBhattacharyya(DefHist* pHM = NULL, DefHist* pHC = NULL, DefHist* pHT = NULL)
{
if(pHM==NULL) pHM = &m_HistModel;
double calcBhattacharyya(DefHist* pHM = NULL, DefHist* pHC = NULL, DefHist* pHT = NULL)
{
if(pHM==NULL) pHM = &m_HistModel;
cvMul(pHM->m_pHist,pHC->m_pHist,pHT->m_pHist);
cvPow(pHT->m_pHist,pHT->m_pHist,0.5);
return cvSum(pHT->m_pHist).val[0] / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
#else
cvMul(pHM->m_pHist,pHC->m_pHist,pHT->m_pHist);
cvPow(pHT->m_pHist,pHT->m_pHist,0.5);
return cvSum(pHT->m_pHist).val[0] / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
#else
- //double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, float x, float y, DefHist* pHist=NULL)
+ // double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, float x, float y, DefHist* pHist=NULL)
double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob, DefHist* pHist=NULL, int /*thread_number*/ = 0)
{
if(pHist==NULL)pHist = &m_HistTemp;
CollectHist(pImg, pImgFG, pBlob, pHist);
return calcBhattacharyya(&m_HistModel, pHist, pHist);
}
double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob, DefHist* pHist=NULL, int /*thread_number*/ = 0)
{
if(pHist==NULL)pHist = &m_HistTemp;
CollectHist(pImg, pImgFG, pBlob, pHist);
return calcBhattacharyya(&m_HistModel, pHist, pHist);
}
void UpdateModelHist(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob)
{
if(m_Alpha>0 && !m_Collision)
void UpdateModelHist(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob)
{
if(m_Alpha>0 && !m_Collision)
CollectHist(pImg, pImgFG, pBlob, &m_HistCandidate);
m_HistModel.Update(&m_HistCandidate, m_Alpha);
CollectHist(pImg, pImgFG, pBlob, &m_HistCandidate);
m_HistModel.Update(&m_HistCandidate, m_Alpha);
m_FGWeight = 2;
AddParam("FGWeight", &m_FGWeight);
CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
m_Alpha = 0.01f;
AddParam("Alpha", &m_Alpha);
m_FGWeight = 2;
AddParam("FGWeight", &m_FGWeight);
CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
m_Alpha = 0.01f;
AddParam("Alpha", &m_Alpha);
m_IterNum = 10;
AddParam("IterNum", &m_IterNum);
CommentParam("IterNum","Maximal number of iteration in meanshift operation");
m_IterNum = 10;
AddParam("IterNum", &m_IterNum);
CommentParam("IterNum","Maximal number of iteration in meanshift operation");
m_KernelMeanShift = NULL;
ReAllocHist(3,5); /* 3D hist, each dim has 2^5 bins*/
}
m_KernelMeanShift = NULL;
ReAllocHist(3,5); /* 3D hist, each dim has 2^5 bins*/
}
if(m_KernelHist) cvReleaseMat(&m_KernelHist);
if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
}
if(m_KernelHist) cvReleaseMat(&m_KernelHist);
if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
}
virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
{
int w = cvRound(CV_BLOB_WX(pBlobInit));
virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
{
int w = cvRound(CV_BLOB_WX(pBlobInit));
CollectHist(pImg, pImgFG, pBlobInit, &m_HistModel);
m_Blob = pBlobInit[0];
};
CollectHist(pImg, pImgFG, pBlobInit, &m_HistModel);
m_Blob = pBlobInit[0];
};
int w = cvRound(m_Blob.w);
int h = cvRound(m_Blob.h);
if( w != m_ObjSize.width || h!=m_ObjSize.height)
int w = cvRound(m_Blob.w);
int h = cvRound(m_Blob.h);
if( w != m_ObjSize.width || h!=m_ObjSize.height)
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y,0);
unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y,0)):NULL;
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y,0);
unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y,0)):NULL;
if(m_HistModel.m_HistVolume>0)
HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
if(m_HistModel.m_HistVolume>0)
HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
if(m_HistCandidate.m_HistVolume>0)
HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
if(m_HistCandidate.m_HistVolume>0)
HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
K = *(DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],yk,xk,sizeof(DefHistType));
if(HC>0)
K = *(DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],yk,xk,sizeof(DefHistType));
if(HC>0)
CV_IMAGE_ELEM(pWFG,uchar,y,x) = (uchar)Vi;
}
CV_IMAGE_ELEM(pWFG,uchar,y,x) = (uchar)Vi;
}
//cvNamedWindow("MSFG_W",0);
//cvShowImage("MSFG_W",pW);
//cvNamedWindow("MSFG_WFG",0);
//cvNamedWindow("MSFG_W",0);
//cvShowImage("MSFG_W",pW);
//cvNamedWindow("MSFG_WFG",0);
//cvSaveImage("MSFG_W.bmp",pW);
//cvSaveImage("MSFG_WFG.bmp",pWFG);
//cvSaveImage("MSFG_FG.bmp",pImgFG);
//cvSaveImage("MSFG_W.bmp",pW);
//cvSaveImage("MSFG_WFG.bmp",pWFG);
//cvSaveImage("MSFG_FG.bmp",pImgFG);
if((x0+m_ObjSize.width)>=pImg->width) x0=pImg->width-m_ObjSize.width-1;
if((y0+m_ObjSize.height)>=pImg->height) y0=pImg->height-m_ObjSize.height-1;
if(x0<0){ x0=0;}
if(y0<0){ y0=0;}
if((x0+m_ObjSize.width)>=pImg->width) x0=pImg->width-m_ObjSize.width-1;
if((y0+m_ObjSize.height)>=pImg->height) y0=pImg->height-m_ObjSize.height-1;
if(x0<0){ x0=0;}
if(y0<0){ y0=0;}
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
DefHistType* pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],y,0,sizeof(DefHistType));
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
DefHistType* pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],y,0,sizeof(DefHistType));
if(m_HistModel.m_HistVolume>0)
HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
if(m_HistModel.m_HistVolume>0)
HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
if(m_HistCandidate.m_HistVolume>0)
HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
if(m_HistCandidate.m_HistVolume>0)
HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
newx = 0.5f*(newx+m_Blob.x);
newy = 0.5f*(newy+m_Blob.y);
if(fabs(newx-m_Blob.x)<0.1 && fabs(newy-m_Blob.y)<0.1) break;
newx = 0.5f*(newx+m_Blob.x);
newy = 0.5f*(newy+m_Blob.y);
if(fabs(newx-m_Blob.x)<0.1 && fabs(newy-m_Blob.y)<0.1) break;
m_Blob.w = MAX(NewBlob.w,5);
m_Blob.h = MAX(NewBlob.h,5);
break;
m_Blob.w = MAX(NewBlob.w,5);
m_Blob.h = MAX(NewBlob.h,5);
break;
virtual double GetConfidence(CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL, IplImage* pImgUnusedReg = NULL)
{
double S = 0.2;
double B = GetBhattacharyya(pImg, pImgUnusedReg, pBlob, &m_HistTemp);
return exp((B-1)/(2*S));
virtual double GetConfidence(CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL, IplImage* pImgUnusedReg = NULL)
{
double S = 0.2;
double B = GetBhattacharyya(pImg, pImgUnusedReg, pBlob, &m_HistTemp);
return exp((B-1)/(2*S));
#if 0
void CvBlobTrackerOneMSFG::CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
#if 0
void CvBlobTrackerOneMSFG::CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
DefHistType* pKernelData = NULL;
{
unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
DefHistType* pKernelData = NULL;
if(UsePrecalculatedKernel)
{
pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
}
if(UsePrecalculatedKernel)
{
pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
}
cvWriteStruct(fs,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd",m_ParticleNum);
cvWriteStruct(fs,"ParticlesResampled",m_pParticlesResampled,"ffffiffd",m_ParticleNum);
};
cvWriteStruct(fs,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd",m_ParticleNum);
cvWriteStruct(fs,"ParticlesResampled",m_pParticlesResampled,"ffffiffd",m_ParticleNum);
};
if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
m_pParticlesPredicted = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
m_pParticlesResampled = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
m_pParticlesPredicted = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
m_pParticlesResampled = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
IplImage* pI = cvCloneImage(pImg);
int h,hN = m_ParticleNum;
CvBlob C = cvBlob(0,0,0,0);
double WS = 0;
IplImage* pI = cvCloneImage(pImg);
int h,hN = m_ParticleNum;
CvBlob C = cvBlob(0,0,0,0);
double WS = 0;
cvEllipse( pI,
cvPointFrom32f(CV_BLOB_CENTER(&C)),
cvSize(cvRound(C.w*0.5),cvRound(C.h*0.5)),
0, 0, 360,
CV_RGB(0,0,255), 1 );
cvEllipse( pI,
cvPointFrom32f(CV_BLOB_CENTER(&C)),
cvSize(cvRound(C.w*0.5),cvRound(C.h*0.5)),
0, 0, 360,
CV_RGB(0,0,255), 1 );
cvEllipse( pI,
cvPointFrom32f(CV_BLOB_CENTER(&m_Blob)),
cvSize(cvRound(m_Blob.w*0.5),cvRound(m_Blob.h*0.5)),
0, 0, 360,
CV_RGB(0,255,0), 1 );
cvEllipse( pI,
cvPointFrom32f(CV_BLOB_CENTER(&m_Blob)),
cvSize(cvRound(m_Blob.w*0.5),cvRound(m_Blob.h*0.5)),
0, 0, 360,
CV_RGB(0,255,0), 1 );
//printf("Blob %d, point (%.1f,%.1f) size (%.1f,%.1f)\n",m_Blob.ID,m_Blob.x,m_Blob.y,m_Blob.w,m_Blob.h);
//printf("Blob %d, point (%.1f,%.1f) size (%.1f,%.1f)\n",m_Blob.ID,m_Blob.x,m_Blob.y,m_Blob.w,m_Blob.h);
//double t;
float r[5];
CvMat rm = cvMat(1,5,CV_32F,r);
//double t;
float r[5];
CvMat rm = cvMat(1,5,CV_32F,r);
m_pParticlesPredicted[p].blob.x += m_pParticlesPredicted[p].Vx;
m_pParticlesPredicted[p].blob.y += m_pParticlesPredicted[p].Vy;
}
else
m_pParticlesPredicted[p].blob.x += m_pParticlesPredicted[p].Vx;
m_pParticlesPredicted[p].blob.y += m_pParticlesPredicted[p].Vy;
}
else
float S = (m_Blob.w + m_Blob.h)*0.5f;
m_pParticlesPredicted[p].blob.x += m_PosVar*S*r[0];
m_pParticlesPredicted[p].blob.y += m_PosVar*S*r[1];
float S = (m_Blob.w + m_Blob.h)*0.5f;
m_pParticlesPredicted[p].blob.x += m_PosVar*S*r[0];
m_pParticlesPredicted[p].blob.y += m_PosVar*S*r[1];
m_pParticlesPredicted[p].Vx += (float)(m_PosVar*S*0.1*r[3]);
m_pParticlesPredicted[p].Vy += (float)(m_PosVar*S*0.1*r[4]);
}
m_pParticlesPredicted[p].Vx += (float)(m_PosVar*S*0.1*r[3]);
m_pParticlesPredicted[p].Vy += (float)(m_PosVar*S*0.1*r[4]);
}
m_pParticlesPredicted[p].blob.w *= (1+m_SizeVar*r[2]);
m_pParticlesPredicted[p].blob.h *= (1+m_SizeVar*r[2]);
m_pParticlesPredicted[p].blob.w *= (1+m_SizeVar*r[2]);
m_pParticlesPredicted[p].blob.h *= (1+m_SizeVar*r[2]);
if(m_pParticlesPredicted[p].blob.w > m_ImgSize.width*0.5f)
{
m_pParticlesPredicted[p].blob.w = m_ImgSize.width*0.5f;
}
if(m_pParticlesPredicted[p].blob.w > m_ImgSize.width*0.5f)
{
m_pParticlesPredicted[p].blob.w = m_ImgSize.width*0.5f;
}
if(m_pParticlesPredicted[p].blob.h > m_ImgSize.height*0.5f)
{
m_pParticlesPredicted[p].blob.h = m_ImgSize.height*0.5f;
}
if(m_pParticlesPredicted[p].blob.h > m_ImgSize.height*0.5f)
{
m_pParticlesPredicted[p].blob.h = m_ImgSize.height*0.5f;
}
#pragma omp parallel for num_threads(m_ThreadNum),schedule(runtime)
#endif
for(p=0;p<m_ParticleNum;++p)
#pragma omp parallel for num_threads(m_ThreadNum),schedule(runtime)
#endif
for(p=0;p<m_ParticleNum;++p)
if(p2>=m_ParticleNum)p2=m_ParticleNum-1;
m_pParticlesResampled[p] = m_pParticlesPredicted[p2];
m_pParticlesResampled[p].W = 1;
if(p2>=m_ParticleNum)p2=m_ParticleNum-1;
m_pParticlesResampled[p] = m_pParticlesPredicted[p2];
m_pParticlesResampled[p].W = 1;
int w = cvRound(m_Blob.w);
int h = cvRound(m_Blob.h);
if( w != m_ObjSize.width || h!=m_ObjSize.height)
{
ReAllocKernel(w,h);
int w = cvRound(m_Blob.w);
int h = cvRound(m_Blob.h);
if( w != m_ObjSize.width || h!=m_ObjSize.height)
{
ReAllocKernel(w,h);
fprintf(stderr, "PF Resampling, %d ticks\n", (int)ticks);
#endif
fprintf(stderr, "PF Resampling, %d ticks\n", (int)ticks);
#endif