空间直方图Meanshift跟踪—代码

typedef struct _TARGETHIST{floatfHist[NBINS];IVT_POINT   pPonitMean[NBINS];//均值doubledCov[4][NBINS];//协方差}TargetHist,*pTargetHist;typedef struct Object{IVT_RECTpTrackWindow;TargetHistsHist;IVT_POINTnCenter;}Object,*pObject;

class ObjectTracking{public:ObjectTracking(void);~ObjectTracking(void);void<span style="white-space:pre"></span>Init(int m_nWidth, int m_nHeight);intm_nWidth;intm_nHeight;float<span style="white-space:pre"></span> GaussHist(TargetHist* pHist1, TargetHist* pHist2);intm_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);voidevalKernel (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;floatpCov1_Temp[4],pCov2_Temp[4];intpMeanSub[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){floatfTemp;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){floatfCOMHist=0.0,fDis;inti,j,nWidth=pTarget->pTrackWindow.nRight-pTarget->pTrackWindow.nLeft;intnHeight = pTarget->pTrackWindow.nBottom - pTarget->pTrackWindow.nTop;intv_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;floatpTemp5[4],fV_b[2],fTemp6[2],fTemp7, fAI_b,fArf[HISTOGRAM_LENGTH];floatpCov1_Temp[4],pCov2_Temp[4];intpMeanSub[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){inthalf_x = nWidth>>1;inthalf_y = nHeight>>1;floateuclideanDistance;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;}}}