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;
}
}
}
