基于C++与粒子滤波算法实现多目标实时跟踪

#include "multtracker.h" MultTracker::MultTracker() { } float MultTracker::computeOverLap(cv::Rect &r1, cv::Rect &r2) { int x1 = r1.x; int y1 = r1.y; int width1 = r1.width; int height1 = r1.height; int x2 = r2.x; int y2 = r2.y; int width2 = r2.width; int height2 = r2.height; int endx = std::max(x1+width1,x2+width2); int startx = std::min(x1,x2); int width = width1+width2-(endx-startx); int endy = std::max(y1+height1,y2+height2); int starty = std::min(y1,y2); int height = height1+height2-(endy-starty); float ratio = 0.0f; float Area,Area1,Area2; if (width<=0||height<=0) return 0.0f; else { Area = width*height; Area1 = width1*height1; Area2 = width2*height2; // ratio = Area /(Area1+Area2-Area); float ratio1 = (Area+0.0001)/Area1; float ratio2 = (Area+0.0001)/Area2; ratio = std::max(ratio1,ratio2); } std::cout << ratio << " ratio:" << std::endl; return ratio; } void MultTracker::CalcuColorHistogram(int x0, int y0, int Wx, int Hy, unsigned char *image, int W, int H, float *ColorHist, int bins) { int x_begin, y_begin; /* 指定图像区域的左上角坐标 */ int y_end, x_end; int x, y, i, index; int r, g, b; float k, r2, f; int a2; for ( i = 0; i < bins; i++ ) /* 直方图各个值赋0 */ ColorHist[i] = 0.0; if ( ( x0 < 0 ) || (x0 >= W) || ( y0 < 0 ) || ( y0 >= H ) || ( Wx <= 0 ) || ( Hy <= 0 ) ) return; x_begin = x0 - Wx; /* 计算实际高宽和区域起始点 */ y_begin = y0 - Hy; if ( x_begin < 0 ) x_begin = 0; if ( y_begin < 0 ) y_begin = 0; x_end = x0 + Wx; y_end = y0 + Hy; if ( x_end >= W ) x_end = W-1;//超出范围的话就用画的框的边界来赋值粒子的区域 if ( y_end >= H ) y_end = H-1; a2 = Wx*Wx+Hy*Hy; /* 计算半径平方a^2 */ f = 0.0; /* 归一化系数 */ for ( y = y_begin; y <= y_end; y++ ) for ( x = x_begin; x <= x_end; x++ ) { r = image[(y*W+x)*3] >> R_SHIFT; /* 计算直方图 */ g = image[(y*W+x)*3+1] >> G_SHIFT; /*移位位数根据R、G、B条数 */ b = image[(y*W+x)*3+2] >> B_SHIFT; index = r * G_BIN * B_BIN + g * B_BIN + b;//把当前rgb换成一个索引 r2 = (float)(((y-y0)*(y-y0)+(x-x0)*(x-x0))*1.0/a2); /* 计算半径平方r^2 */ k = 1 - r2; /* k(r) = 1-r^2, |r| < 1; 其他值 k(r) = 0 ，影响力*/ f = f + k; ColorHist[index] = ColorHist[index] + k; /* 计算核密度加权彩色直方图 */ } for ( i = 0; i < bins; i++ ) /* 归一化直方图 */ ColorHist[i] = ColorHist[i]/f; return; } float MultTracker::randGaussian(float u, float sigma) { float x1, x2, v1, v2; float s = 100.0; float y; /* 使用筛选法产生正态分布N(0,1)的随机数(Box-Mulles方法) 1. 产生[0,1]上均匀随机变量X1,X2 2. 计算V1=2*X1-1,V2=2*X2-1,s=V1^2+V2^2 3. 若s<=1,转向步骤4，否则转1 4. 计算A=(-2ln(s)/s)^(1/2),y1=V1*A, y2=V2*A y1,y2为N(0,1)随机变量 */ while ( s > 1.0 ) { x1 = rand0_1(); x2 = rand0_1(); v1 = 2 * x1 - 1; v2 = 2 * x2 - 1; s = v1*v1 + v2*v2; } y = (float)(sqrt( -2.0 * log(s)/s ) * v1); /* 根据公式 z = sigma * y + u 将y变量转换成N(u,sigma)分布 */ return( sigma * y + u ); } void MultTracker::ImportanceSampling(float *c, int *ResampleIndex, int N) { float rnum, * cumulateWeight; int i, j; cumulateWeight = new float [N+1]; /* 申请累计权重数组内存，大小为N+1 */ NormalizeCumulatedWeight( c, cumulateWeight, N ); /* 计算累计权重 */ for ( i = 0; i < N; i++ ) { rnum = rand0_1(); /* 随机产生一个[0,1]间均匀分布的数 */ j = BinearySearch( rnum, cumulateWeight, N+1 ); /* 搜索<=rnum的最小索引j */ if ( j == N ) j--; ResampleIndex[i] = j; /* 放入重采样索引数组 */ } delete[] cumulateWeight; return; } void MultTracker::ReSelect(SPACESTATE *state, float *weight, int N) { SPACESTATE * tmpState;//新的放狗的地方 int i, * rsIdx;//统计的随机数所掉区间的索引 tmpState = new SPACESTATE[N]; rsIdx = new int[N]; ImportanceSampling( weight, rsIdx, N ); /* 根据权重重新采样 */ for ( i = 0; i < N; i++ ) tmpState[i] = state[rsIdx[i]];//temState为临时变量,其中state[i]用state[rsIdx[i]]来代替 for ( i = 0; i < N; i++ ) state[i] = tmpState[i]; delete[] tmpState; delete[] rsIdx; return; } void MultTracker::Propagate(SPACESTATE *state, int N, cv::Mat &trackImg) { int i; int j; float rn[7]; /* 对每一个状态向量state[i](共N个)进行更新 */ for ( i = 0; i < N; i++ ) /* 加入均值为0的随机高斯噪声 */ { for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 产生7个随机高斯分布的数 */ state[i].xt = (int)(state[i].xt + state[i].v_xt * DELTA_T + rn[0] * state[i].Hxt + 0.5); state[i].yt = (int)(state[i].yt + state[i].v_yt * DELTA_T + rn[1] * state[i].Hyt + 0.5); state[i].v_xt = (float)(state[i].v_xt + rn[2] * VELOCITY_DISTURB); state[i].v_yt = (float)(state[i].v_yt + rn[3] * VELOCITY_DISTURB); state[i].Hxt = (int)(state[i].Hxt+state[i].Hxt*state[i].at_dot + rn[4] * SCALE_DISTURB + 0.5); state[i].Hyt = (int)(state[i].Hyt+state[i].Hyt*state[i].at_dot + rn[5] * SCALE_DISTURB + 0.5); state[i].at_dot = (float)(state[i].at_dot + rn[6] * SCALE_CHANGE_D); // cv::circle(trackImg,cv::Point(state[i].xt,state[i].yt),3,cv::Scalar(0,255,0),1,8,3); } return; } void MultTracker::Observe(SPACESTATE *state, float *weight, int N, unsigned char *image, int W, int H, float *ObjectHist, int hbins) { int i; float * ColorHist; float rho; ColorHist = new float[hbins]; for ( i = 0; i < N; i++ ) { /* (1) 计算彩色直方图分布 */ CalcuColorHistogram( state[i].xt, state[i].yt,state[i].Hxt, state[i].Hyt, image, W, H, ColorHist, hbins ); /* (2) Bhattacharyya系数 */ rho = CalcuBhattacharyya( ColorHist, ObjectHist, hbins ); /* (3) 根据计算得的Bhattacharyya系数计算各个权重值 */ weight[i] = CalcuWeightedPi( rho ); } delete[] ColorHist; return; } void MultTracker::Estimation(SPACESTATE *state, float *weight, int N, SPACESTATE &EstState) { int i; float at_dot, Hxt, Hyt, v_xt, v_yt, xt, yt; float weight_sum; at_dot = 0; Hxt = 0; Hyt = 0; v_xt = 0; v_yt = 0; xt = 0; yt = 0; weight_sum = 0; for ( i = 0; i < N; i++ ) /* 求和 */ { at_dot += state[i].at_dot * weight[i]; Hxt += state[i].Hxt * weight[i]; Hyt += state[i].Hyt * weight[i]; v_xt += state[i].v_xt * weight[i]; v_yt += state[i].v_yt * weight[i]; xt += state[i].xt * weight[i]; yt += state[i].yt * weight[i]; weight_sum += weight[i]; } /* 求平均 */ if ( weight_sum <= 0 ) weight_sum = 1; /* 防止被0除，一般不会发生 */ EstState.at_dot = at_dot/weight_sum; EstState.Hxt = (int)(Hxt/weight_sum); EstState.Hyt = (int)(Hyt/weight_sum); EstState.v_xt = v_xt/weight_sum; EstState.v_yt = v_yt/weight_sum; EstState.xt = (int)(xt/weight_sum); EstState.yt = (int)(yt/weight_sum); return; } void MultTracker::ModelUpdate(SPACESTATE EstState, float *TargetHist, int bins, float PiT, unsigned char *img, int W, int H) { float * EstHist, Bha, Pi_E; int i; EstHist = new float [bins]; /* (1)在估计值处计算目标直方图 */ CalcuColorHistogram( EstState.xt, EstState.yt, EstState.Hxt, EstState.Hyt, img, W, H, EstHist, bins ); /* (2)计算Bhattacharyya系数 */ Bha = CalcuBhatta