空间直方图Meanshift跟踪—代码

typedef struct _TARGETHIST
{
float		fHist[NBINS];
IVT_POINT   pPonitMean[NBINS];		//均值
double		dCov[4][NBINS];			//协方差
}TargetHist,*pTargetHist;

typedef struct Object
{
IVT_RECT			pTrackWindow;
TargetHist			sHist;
IVT_POINT			nCenter;
}Object,*pObject;

class ObjectTracking
{
public:
ObjectTracking(void);
~ObjectTracking(void);
void	<span style="white-space:pre">	</span>Init(int m_nWidth, int m_nHeight);
int		m_nWidth;
int		m_nHeight;
float	<span style="white-space:pre">	</span> GaussHist(TargetHist* pHist1, TargetHist* pHist2);
int		m_nThresh;
void	<span style="white-space:pre">	</span>CreateHist(int* pSource, IVT_RECT* pBlob ,TargetHist *pHist);
void<span style="white-space:pre">	</span>	MatrixInvert(float* pMatrix, float* pMatrixIn);
float	<span style="white-space:pre">	</span>MeanshiftTracking(int	*pFrame,Object *pTarget,IVT_RECT *pSearch);
float	 <span style="white-space:pre">	</span>FindLocation(int* pImg,TargetHist *pBlob, TargetHist *pSearch,IVT_RECT pSearchRect,int &X, int &Y,int nHalfWidth, int nHalfHeight);
void	evalKernel (float*** kArray,int*** nDevi,int nWidth,int nHeight);
void <span style="white-space:pre">	</span>ReleseObject(void);
};

/************************************************************************/
/* 计算直方图相似度                                                     */
/************************************************************************/
float ObjectTracking::GaussHist(TargetHist* pHist1, TargetHist* pHist2)
{
float   pCov1_Invert[4], pCov2_Invert[4], fTemp1[2],pCovAdd[4], fTemp2[2], fTemp3, fTemp4,fSum=0, fTemp5;
float	pCov1_Temp[4],pCov2_Temp[4];
int		pMeanSub[2],i,j;
for (i=0; i<20; i++)
{
pMeanSub[0] = pHist1->pPonitMean[i].x - pHist2->pPonitMean[i].x;
pMeanSub[1] = pHist1->pPonitMean[i].y - pHist2->pPonitMean[i].y;

for (j=0; j<4; j++)
{
pCov1_Temp[j] = pHist1->dCov[j][i];
pCov2_Temp[j] = pHist2->dCov[j][i];
}

MatrixInvert(pCov1_Temp, pCov1_Invert);
MatrixInvert(pCov2_Temp, pCov2_Invert);

pCovAdd[0] = pCov1_Invert[0] + pCov2_Invert[0];
pCovAdd[1] = pCov1_Invert[1] + pCov2_Invert[1];
pCovAdd[2] = pCov1_Invert[2] + pCov2_Invert[2];
pCovAdd[3] = pCov1_Invert[3] + pCov2_Invert[3];

fTemp2[0] = pMeanSub[0]*pCovAdd[0] + pMeanSub[1]*pCovAdd[2];
fTemp2[1] = pMeanSub[0]*pCovAdd[1] + pMeanSub[1]*pCovAdd[3];

fTemp3 = fTemp2[0]*pMeanSub[0]+fTemp2[1]*pMeanSub[1];

fTemp4 = exp((-0.5)*fTemp3);
fTemp5 = sqrt((float)(pHist1->fHist[i]*pHist2->fHist[i]));

fSum += fTemp4*fTemp5;
}
return fSum;
}

/************************************************************************/
/* 创建直方图                                                           */
/************************************************************************/
void ObjectTracking::CreateHist(int* pSource,  IVT_RECT* pBlob, TargetHist *pHist)
{
int      i,j,ntemp,nSub,nHistCount[NBINS],*nBlobImg,nWidth,nHeight,n,m;
double	 dRange,dTemp;
float	 fTotal = 0.0;
float	**pWeight = NULL;
int		**pArry = NULL;
int       nMax=0, nMin=65535;
for (i=pBlob->nTop; i<pBlob->nBottom; i++)
{
for (j=pBlob->nLeft; j<pBlob->nRight; j++)
{
nMax = nMax>pSource[i*m_nWidth+j]?nMax:pSource[i*m_nWidth+j];
nMin = nMin<pSource[i*m_nWidth+j]?nMin:pSource[i*m_nWidth+j];
}
}
//dRange = (double)(MAXBIN-MINBIN) / NBINS;
dRange = (double)(nMax-nMin) / NBINS;
nWidth = pBlob->nRight - pBlob->nLeft;
nHeight = pBlob->nBottom - pBlob->nTop;
memset(pHist->fHist, 0, sizeof(float)*NBINS);
memset(pHist->dCov, 0, sizeof(double)*NBINS*4);
memset(pHist->pPonitMean,0, sizeof(IVT_POINT)*NBINS);
memset(nHistCount, 0, sizeof(int)*NBINS);

pWeight = new float*[nWidth];
ZeroMemory(pWeight, nWidth * sizeof(float*));
for (i=0; i<nWidth; i++)
{
pWeight[i] = new float[nHeight];
ZeroMemory(pWeight[i], nHeight* sizeof(float));
}
pArry = new int*[nWidth];
ZeroMemory(pArry, nWidth * sizeof(int*));
for (i=0; i<nWidth; i++)
{
pArry[i] = new int[nHeight];
ZeroMemory(pArry[i], nHeight* sizeof(int));
}
nBlobImg = (int*)malloc(nWidth*nHeight*sizeof(int));

evalKernel(&pWeight,&pArry,nWidth, nHeight);

for (i=pBlob->nTop,n=0; i<pBlob->nBottom; i++,n++)
{
for (j=pBlob->nLeft,m=0; j<pBlob->nRight; j++,m++)
{
nSub = pSource[i*m_nWidth+j] - nMin;
if (nSub<=0)
{
ntemp = 0;
}
else
{
ntemp = ceil((double)nSub / dRange);
if (ntemp>NBINS-1)
{
ntemp = NBINS-1;
}
}
pHist->fHist[ntemp] += pWeight[m]
;;
pHist->pPonitMean[ntemp].x += (j-pBlob->nLeft)*pArry[m]
;
pHist->pPonitMean[ntemp].y += (i-pBlob->nTop)*pArry[m]
;
nHistCount[ntemp] += 1*pArry[m]
;
nBlobImg[(i-pBlob->nTop)*nWidth+j-pBlob->nLeft]=ntemp;
fTotal += pWeight[m]
;
}
}
for(i=0; i<NBINS; i++)
{
if (fTotal != 0)
{
pHist->fHist[i] = pHist->fHist[i]/fTotal;
}
if (nHistCount[i]!=0)
{
pHist->pPonitMean[i].x = pHist->pPonitMean[i].x / nHistCount[i];
pHist->pPonitMean[i].y = pHist->pPonitMean[i].y / nHistCount[i];
}

}
for (i=0; i<nHeight; i++)
{
for (j=0; j<nWidth; j++)
{
ntemp = nBlobImg[i*nWidth+j];
pHist->dCov[0][ntemp] += (j-pHist->pPonitMean[ntemp].x)
*(j-pHist->pPonitMean[ntemp].x) * pArry[j][i];
pHist->dCov[1][ntemp] += (j-pHist->pPonitMean[ntemp].x)
*(i-pHist->pPonitMean[ntemp].y) * pArry[j][i];
pHist->dCov[2][ntemp] += (j-pHist->pPonitMean[ntemp].x)
*(i-pHist->pPonitMean[ntemp].y) * pArry[j][i];
pHist->dCov[3][ntemp] += (i-pHist->pPonitMean[ntemp].y)
*(i-pHist->pPonitMean[ntemp].y) * pArry[j][i];
}
}
for(i=0; i<NBINS; i++)
{
if (nHistCount[i]!=0)
{
pHist->dCov[0][i] = pHist->dCov[0][i] / nHistCount[i];
pHist->dCov[1][i] = pHist->dCov[1][i] / nHistCount[i];;
pHist->dCov[2][i] = pHist->dCov[2][i] / nHistCount[i];
pHist->dCov[3][i] = pHist->dCov[3][i] / nHistCount[i];
}

}
free(nBlobImg);
for (i=0; i<nWidth; i++)
delete[] pWeight[i];
delete [] pWeight;
pWeight = NULL;

for (i=0; i<nWidth; i++)
delete[] pArry[i];
delete []pArry;
pArry = NULL;
}

/************************************************************************/
/* 求逆矩阵2*2                                                           */
/************************************************************************/
void ObjectTracking::MatrixInvert(float* pMatrix, float* pMatrixIn)
{
float	fTemp;
fTemp = pMatrix[0]*pMatrix[3] -  pMatrix[1]*pMatrix[2];
if (fTemp<10e-6 && fTemp>-10e-6)
{
fTemp = 0.0;
}
else
{
fTemp = 1.0 / fTemp;
}
pMatrixIn[0] = pMatrix[3] * fTemp;
pMatrixIn[1] = -pMatrix[1] * fTemp;
pMatrixIn[2] = -pMatrix[2] * fTemp;
pMatrixIn[3] = pMatrix[0] * fTemp;
}
/************************************************************************/
/* meanshift跟踪                                                        */
/************************************************************************/
float ObjectTracking::MeanshiftTracking(int	*pFrame,Object *pTarget,IVT_RECT *pBlob)
{
float		fCOMHist=0.0,fDis;
int			i,j,nWidth=pTarget->pTrackWindow.nRight-pTarget->pTrackWindow.nLeft;
int			nHeight = pTarget->pTrackWindow.nBottom - pTarget->pTrackWindow.nTop;
int			v_x,v_y,nCenter_x,nCenter_y,nHalfWidth, nHalfHeight;
TargetHist	*pSearchHist;
IVT_RECT	 pSearchRect;
pSearchHist = (TargetHist*)malloc(sizeof(TargetHist));
nCenter_x = (pBlob->nRight+pBlob->nLeft)>>1;
nCenter_y = (pBlob->nTop+pBlob->nBottom)>>1;
nHalfHeight = nHeight>>1;
nHalfWidth = nWidth>>1;
pSearchRect.nLeft = nCenter_x - nHalfWidth;
pSearchRect.nTop = nCenter_y - nHalfHeight;
pSearchRect.nBottom =  nCenter_y + nHalfHeight;
pSearchRect.nRight = nCenter_x + nHalfWidth;
for (i=0; i<MEANSHIFT_ITARATION_NO; i++)
{
CreateHist(pFrame,&pSearchRect, pSearchHist);
FindLocation(pFrame,&pTarget->sHist, pSearchHist,pSearchRect,v_x, v_y,nHalfWidth,nHalfHeight);
pSearchRect.nTop = v_y+pSearchRect.nTop;
pSearchRect.nBottom = pSearchRect.nBottom+v_y;
pSearchRect.nLeft = v_x+pSearchRect.nLeft;
pSearchRect.nRight = pSearchRect.nRight+v_x;
nCenter_x = nCenter_x+v_x;
nCenter_y = nCenter_y+v_y;
fDis = v_x*v_x +v_y*v_y;
if (fDis<2)
{
break;
}
}

nHalfWidth = (pBlob->nRight - pBlob->nLeft)>>1;
nHalfHeight = (pBlob->nBottom - pBlob->nTop)>>1;
pTarget->nCenter.x = nCenter_x;
pTarget->nCenter.y = nCenter_y;
pTarget->pTrackWindow.nTop = nCenter_y - nHalfHeight;
pTarget->pTrackWindow.nBottom = nCenter_y + nHalfHeight;
pTarget->pTrackWindow.nLeft = nCenter_x - nHalfWidth;
pTarget->pTrackWindow.nRight = nCenter_x + nHalfWidth;
CreateHist(pFrame, &pTarget->pTrackWindow, &pTarget->sHist);

free(pSearchHist);
return fCOMHist;
}

/************************************************************************/
/* 目标定位                                                                 */
/************************************************************************/
float ObjectTracking::FindLocation(int* pImg,TargetHist *pBlob, TargetHist *pSearch,IVT_RECT pSearchRect,int &X, int &Y,int nHalfWidth, int nHalfHeight)
{
float   pCov1_Invert[4], pCov2_Invert[4], fTemp1[2],pCovAdd[4], fTemp2[2], fTemp3, fTemp4,fSum=0;
float	pTemp5[4],fV_b[2],fTemp6[2],fTemp7, fAI_b,fArf[HISTOGRAM_LENGTH];
float	pCov1_Temp[4],pCov2_Temp[4];
int		pMeanSub[2],i,j,nWidth,nHeight;
int     nMin=65535,nMax = 0;
memset(fV_b, 0, sizeof(float)*2);
memset(fArf, 0, sizeof(float)*HISTOGRAM_LENGTH);
for (i=0; i<NBINS; i++)
{
pMeanSub[0] = pBlob->pPonitMean[i].x - pSearch->pPonitMean[i].x;
pMeanSub[1] = pBlob->pPonitMean[i].y - pSearch->pPonitMean[i].y;

for (j=0; j<4; j++)
{
pCov1_Temp[j] = pBlob->dCov[j][i];
pCov2_Temp[j] = pSearch->dCov[j][i];
}

MatrixInvert(pCov1_Temp, pCov1_Invert);
MatrixInvert(pCov2_Temp, pCov2_Invert);

pCovAdd[0] = pCov1_Invert[0] + pCov2_Invert[0];
pCovAdd[1] = pCov1_Invert[1] + pCov2_Invert[1];
pCovAdd[2] = pCov1_Invert[2] + pCov2_Invert[2];
pCovAdd[3] = pCov1_Invert[3] + pCov2_Invert[3];

fTemp2[0] = pMeanSub[0]*pCovAdd[0] + pMeanSub[1]*pCovAdd[2];
fTemp2[1] = pMeanSub[0]*pCovAdd[1] + pMeanSub[1]*pCovAdd[3];

fTemp3 = fTemp2[0]*pMeanSub[0]+fTemp2[1]*pMeanSub[1];

fAI_b = exp((-0.5)*fTemp3);
fTemp4 = sqrt(pBlob->fHist[i]*pSearch->fHist[i]);

fTemp7 = fTemp4*fAI_b;
fSum += fTemp7;
if (pSearch->fHist[i] != 0)
{
fArf[i] += fAI_b * sqrt(pBlob->fHist[i]/pSearch->fHist[i]);
}

pTemp5[0] = fTemp7* pCovAdd[0];
pTemp5[1] = fTemp7* pCovAdd[1];
pTemp5[2] = fTemp7* pCovAdd[2];
pTemp5[3] = fTemp7* pCovAdd[3];

fTemp6[0] = pTemp5[0]*pMeanSub[0] + pTemp5[1]*pMeanSub[1];
fTemp6[1] = pTemp5[2]*pMeanSub[0] + pTemp5[3]*pMeanSub[1];

fV_b[0] += fTemp6[0];
fV_b[1] += fTemp6[1];
}
int      ntemp,nSub,nCount[20]={0},m,n,fi,fj;
float	 dRange,fSum_y=0.0,fSum_x=0.0,fSum_Arf=0.0;
for (i=pSearchRect.nTop;i<pSearchRect.nBottom; i++)
{
for (j=pSearchRect.nLeft; j<pSearchRect.nRight; j++)
{
nMax = nMax>pImg[i*m_nWidth+j]?nMax:pImg[i*m_nWidth+j];
nMin = nMin<pImg[i*m_nWidth+j]?nMin:pImg[i*m_nWidth+j];
}
}
//dRange = (double)(MAXBIN-MINBIN) / 20.0;
dRange = (double)(nMax-nMin) / 20.0;
for (i=pSearchRect.nTop,m=0,fi=-nHalfHeight; i<pSearchRect.nBottom; i++,m++,fi++)
{
for (j=pSearchRect.nLeft,n=0,fj=-nHalfWidth; j<pSearchRect.nRight; j++,n++,fj++)
{
nSub = pImg[i*m_nWidth+j] - nMin;
if (nSub<=0)
{
ntemp = 0;
}
else
{
ntemp = ceil((double)nSub / dRange);
if (ntemp>19)
{
ntemp = 19;
}
}

fSum_y += fArf[ntemp]*fi;
fSum_x += fArf[ntemp]*fj;
fSum_Arf += fArf[ntemp];
nCount[ntemp]++;
}
}

X = (fSum_x-fV_b[0]) / fSum_Arf;
Y = (fSum_y-fV_b[1]) / fSum_Arf;

return fSum;
}

void ObjectTracking::evalKernel (float*** kArray,int*** nDevi,int nWidth,int nHeight)
{
int		half_x = nWidth>>1;
int		half_y = nHeight>>1;
float	euclideanDistance;
for (int x = -half_x; x<half_x; x++)
{
for (int y =-half_y; y<half_y; y++)
{
euclideanDistance = sqrt( pow( ( (double)(x)/(double)(half_x) ) ,2.0)
+pow( ( (double)(y)/(double)(half_y) ) ,2.0) );
if (euclideanDistance > 1)
(*nDevi)[x+half_x][y+half_y] = 0;
else
(*nDevi)[x+half_x][y+half_y] = 1;
(*kArray)[x+half_x][y+half_y] = euclideanDistance;
}
}
}