空间直方图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][n];;
			pHist->pPonitMean[ntemp].x += (j-pBlob->nLeft)*pArry[m][n];
			pHist->pPonitMean[ntemp].y += (i-pBlob->nTop)*pArry[m][n];
			nHistCount[ntemp] += 1*pArry[m][n];
			nBlobImg[(i-pBlob->nTop)*nWidth+j-pBlob->nLeft]=ntemp;
			fTotal += pWeight[m][n];
		}
	}
	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;
		}
	}
}