基于粒子滤波的物体跟踪

一直都觉得粒子滤波是个挺牛的东西,每次试图看文献都被复杂的数学符号搞得看不下去。一个偶然的机会发现了Rob Hess(http://web.engr.oregonstate.edu/~hess/)实现的这个粒子滤波。从代码入手,一下子就明白了粒子滤波的原理。根据维基百科上对粒子滤波的介绍(http://en.wikipedia.org/wiki/Particle_filter),粒子滤波其实有很多变种,Rob Hess实现的这种应该是最基本的一种,Sampling Importance Resampling (SIR),根据重要性重采样。下面是我对粒子滤波实现物体跟踪的算法原理的粗浅理解:

1)初始化阶段-提取跟踪目标特征

该阶段要人工指定跟踪目标,程序计算跟踪目标的特征,比如可以采用目标的颜色特征。具体到Rob Hess的代码,开始时需要人工用鼠标拖动出一个跟踪区域,然后程序自动计算该区域色调(Hue)空间的直方图,即为目标的特征。直方图可以用一个向量来表示,所以目标特征就是一个N*1的向量V。

2)搜索阶段-放狗

好,我们已经掌握了目标的特征,下面放出很多条狗,去搜索目标对象,这里的狗就是粒子particle。狗有很多种放法。比如,a)均匀的放:即在整个图像平面均匀的撒粒子(uniform distribution);b)在上一帧得到的目标附近按照高斯分布来放,可以理解成,靠近目标的地方多放,远离目标的地方少放。Rob Hess的代码用的是后一种方法。狗放出去后,每条狗怎么搜索目标呢?就是按照初始化阶段得到的目标特征(色调直方图,向量V)。每条狗计算它所处的位置处图像的颜色特征,得到一个色调直方图,向量Vi,计算该直方图与目标直方图的相似性。相似性有多种度量,最简单的一种是计算sum(abs(Vi-V)).每条狗算出相似度后再做一次归一化,使得所有的狗得到的相似度加起来等于1.

3)决策阶段

我们放出去的一条条聪明的狗向我们发回报告,“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...那么目标究竟最可能在哪里呢?我们做次加权平均吧。设N号狗的图像像素坐标是(Xn,Yn),它报告的相似度是Wn,于是目标最可能的像素坐标X = sum(Xn*Wn),Y = sum(Yn*Wn).

4)重采样阶段Resampling

既然我们是在做目标跟踪,一般说来,目标是跑来跑去乱动的。在新的一帧图像里,目标可能在哪里呢?还是让我们放狗搜索吧。但现在应该怎样放狗呢?让我们重温下狗狗们的报告吧。“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...综合所有狗的报告,一号狗处的相似度最高,三号狗处的相似度最低,于是我们要重新分布警力,正所谓好钢用在刀刃上,我们在相似度最高的狗那里放更多条狗,在相似度最低的狗那里少放狗,甚至把原来那条狗也撤回来。这就是Sampling Importance Resampling,根据重要性重采样(更具重要性重新放狗)。

(2)->(3)->(4)->(2)如是反复循环,即完成了目标的动态跟踪。

 

根据我的粗浅理解,粒子滤波的核心思想是随机采样+重要性重采样。既然我不知道目标在哪里,那我就随机的撒粒子吧。撒完粒子后,根据特征相似度计算每个粒子的重要性,然后在重要的地方多撒粒子,不重要的地方少撒粒子。所以说粒子滤波较之蒙特卡洛滤波,计算量较小。这个思想和RANSAC算法真是不谋而合。RANSAC的思想也是(比如用在最简单的直线拟合上),既然我不知道直线方程是什么,那我就随机的取两个点先算个直线出来,然后再看有多少点符合我的这条直线。哪条直线能获得最多的点的支持,哪条直线就是目标直线。想法非常简单,但效果很好。



Mac+Xcode+openCV:
代码下载地址: here

代码实现:

运行方式:按P停止,在前景窗口鼠标点击目标,会自动生成外接矩形,再次按P,对该选定目标进行跟踪。

//
//  main.cpp
//  opencvLearn
//
//  Created by 刘鹏 on 2016/11/24.
//  Copyright © 2016年 刘鹏. All rights reserved.
//

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
// 新版本写在下面文件中:
#include <opencv2/nonfree/features2d.hpp>
//#include "opencv2/features2d/features2d.hpp"
#include<opencv2/legacy/legacy.hpp>

using namespace std;
using namespace cv;

#define B(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3]		//B
#define G(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+1]	//G
#define R(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)*3+2]	//R
#define S(image,x,y) ((uchar*)(image->imageData + image->widthStep*(y)))[(x)]
#define  Num 10  //帧差的间隔
#define  T 40    //Tf
#define Re 30     //
#define ai 0.08   //学习率

#define CONTOUR_MAX_AREA 10000
#define CONTOUR_MIN_AREA 50

# define R_BIN      8  /* 红色分量的直方图条数 */
# define G_BIN      8  /* 绿色分量的直方图条数 */
# define B_BIN      8  /* 兰色分量的直方图条数 */

# define R_SHIFT    5  /* 与上述直方图条数对应 */
# define G_SHIFT    5  /* 的R、G、B分量左移位数 */
# define B_SHIFT    5  /* log2( 256/8 )为移动位数 */

/*
 采用Park and Miller方法产生[0,1]之间均匀分布的伪随机数
 算法详细描述见:
 [1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING.
 Cambridge University Press. 1992. pp.278-279.
 [2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,
 vol. 31, pp. 1192–1201.
 */

#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define MASK 123459876


typedef struct __SpaceState {  /* 状态空间变量 */
    int xt;               /* x坐标位置 */
    int yt;               /* x坐标位置 */
    float v_xt;           /* x方向运动速度 */
    float v_yt;           /* y方向运动速度 */
    int Hxt;              /* x方向半窗宽 */
    int Hyt;              /* y方向半窗宽 */
    float at_dot;         /* 尺度变换速度 */
} SPACESTATE;


bool pause=false;//是否暂停
bool track = false;//是否跟踪
IplImage *curframe=NULL;
IplImage *pBackImg=NULL;
IplImage *pFrontImg=NULL;
IplImage *pTrackImg =NULL;
unsigned char * img;//把iplimg改到char*  便于计算
int xin,yin;//跟踪时输入的中心点
int xout,yout;//跟踪时得到的输出中心点
int Wid,Hei;//图像的大小
int WidIn,HeiIn;//输入的半宽与半高
int WidOut,HeiOut;//输出的半宽与半高

long ran_seed = 802163120; /* 随机数种子,为全局变量,设置缺省值 */

float DELTA_T = (float)0.05;    /* 帧频,可以为30,25,15,10等 */
int POSITION_DISTURB = 15;      /* 位置扰动幅度   */
float VELOCITY_DISTURB = 40.0;  /* 速度扰动幅值   */
float SCALE_DISTURB = 0.0;      /* 窗宽高扰动幅度 */
float SCALE_CHANGE_D = (float)0.001;   /* 尺度变换速度扰动幅度 */

int NParticle = 75;       /* 粒子个数   */
float * ModelHist = NULL; /* 模型直方图 */
SPACESTATE * states = NULL;  /* 状态数组 */
float * weights = NULL;   /* 每个粒子的权重 */
int nbin;                 /* 直方图条数 */
float Pi_Thres = (float)0.90; /* 权重阈值   */
float Weight_Thres = (float)0.0001;  /* 最大权重阈值,用来判断是否目标丢失 */


/*
 设置种子数
 一般利用系统时间来进行设置,也可以直接传入一个long型整数
 */
long set_seed( long setvalue )
{
    if ( setvalue != 0 ) /* 如果传入的参数setvalue!=0,设置该数为种子 */
        ran_seed = setvalue;
    else                 /* 否则,利用系统时间为种子数 */
    {
        ran_seed = time(NULL);
    }
    return( ran_seed );
}

/*
 计算一幅图像中某个区域的彩色直方图分布
 输入参数:
 int x0, y0:           指定图像区域的中心点
 int Wx, Hy:           指定图像区域的半宽和半高
 unsigned char * image:图像数据,按从左至右,从上至下的顺序扫描,
 颜色排列次序:RGB, RGB, ...
 (或者:YUV, YUV, ...)
 int W, H:             图像的宽和高
 输出参数:
 float * ColorHist:    彩色直方图,颜色索引按:
 i = r * G_BIN * B_BIN + g * B_BIN + b排列
 int bins:             彩色直方图的条数R_BIN*G_BIN*B_BIN(这里取8x8x8=512)
 */
void 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;
    /* 考虑特殊情况:x0, y0在图像外面,或者,Wx<=0, Hy<=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;
            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;
}

/*
 计算Bhattacharyya系数
 输入参数:
 float * p, * q:      两个彩色直方图密度估计
 int bins:            直方图条数
 返回值:
 Bhattacharyya系数
 */
float CalcuBhattacharyya( float * p, float * q, int bins )
{
    int i;
    float rho;
    
    rho = 0.0;
    for ( i = 0; i < bins; i++ )
        rho = (float)(rho + sqrt( p[i]*q[i] ));
    
    return( rho );
}


/*# define RECIP_SIGMA  3.98942280401  / * 1/(sqrt(2*pi)*sigma), 这里sigma = 0.1 * /*/
# define SIGMA2       0.02           /* 2*sigma^2, 这里sigma = 0.1 */

float CalcuWeightedPi( float rho )
{
    float pi_n, d2;
    
    d2 = 1 - rho;
    //pi_n = (float)(RECIP_SIGMA * exp( - d2/SIGMA2 ));
    pi_n = (float)(exp( - d2/SIGMA2 ));
    
    return( pi_n );
}

/*
 采用Park and Miller方法产生[0,1]之间均匀分布的伪随机数
 算法详细描述见:
 [1] NUMERICAL RECIPES IN C: THE ART OF SCIENTIFIC COMPUTING.
 Cambridge University Press. 1992. pp.278-279.
 [2] Park, S.K., and Miller, K.W. 1988, Communications of the ACM,
 vol. 31, pp. 1192–1201.
 */

float ran0(long *idum)
{
    long k;
    float ans;
    
    /* *idum ^= MASK;*/      /* XORing with MASK allows use of zero and other */
    k=(*idum)/IQ;            /* simple bit patterns for idum.                 */
    *idum=IA*(*idum-k*IQ)-IR*k;  /* Compute idum=(IA*idum) % IM without over- */
    if (*idum < 0) *idum += IM;  /* flows by Schrage’s method.               */
    ans=AM*(*idum);          /* Convert idum to a floating result.            */
    /* *idum ^= MASK;*/      /* Unmask before return.                         */
    return ans;
}


/*
 获得一个[0,1]之间均匀分布的随机数
 */
float rand0_1()
{
    return( ran0( &ran_seed ) );
}



/*
 获得一个x - N(u,sigma)Gaussian分布的随机数
 */
float 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 );
}



/*
 初始化系统
 int x0, y0:        初始给定的图像目标区域坐标
 int Wx, Hy:        目标的半宽高
 unsigned char * img:图像数据,RGB形式
 int W, H:          图像宽高
 */
int Initialize( int x0, int y0, int Wx, int Hy,
               unsigned char * img, int W, int H )
{
    int i, j;
    float rn[7];
    
    set_seed( 0 ); /* 使用系统时钟作为种子,这个函数在 */
    /* 系统初始化时候要调用一次,且仅调用1次 */
    //NParticle = 75; /* 采样粒子个数 */
    //Pi_Thres = (float)0.90; /* 设置权重阈值 */
    states = new SPACESTATE [NParticle]; /* 申请状态数组的空间 */
    if ( states == NULL ) return( -2 );
    weights = new float [NParticle];     /* 申请粒子权重数组的空间 */
    if ( weights == NULL ) return( -3 );
    nbin = R_BIN * G_BIN * B_BIN; /* 确定直方图条数 */
    ModelHist = new float [nbin]; /* 申请直方图内存 */
    if ( ModelHist == NULL ) return( -1 );
    
    /* 计算目标模板直方图 */
    CalcuColorHistogram( x0, y0, Wx, Hy, img, W, H, ModelHist, nbin );
    
    /* 初始化粒子状态(以(x0,y0,1,1,Wx,Hy,0.1)为中心呈N(0,0.4)正态分布) */
    states[0].xt = x0;
    states[0].yt = y0;
    states[0].v_xt = (float)0.0; // 1.0
    states[0].v_yt = (float)0.0; // 1.0
    states[0].Hxt = Wx;
    states[0].Hyt = Hy;
    states[0].at_dot = (float)0.0; // 0.1
    weights[0] = (float)(1.0/NParticle); /* 0.9; */
    for ( i = 1; i < NParticle; i++ )
    {
        for ( j = 0; j < 7; j++ ) rn[j] = randGaussian( 0, (float)0.6 ); /* 产生7个随机高斯分布的数 */
        states[i].xt = (int)( states[0].xt + rn[0] * Wx );
        states[i].yt = (int)( states[0].yt + rn[1] * Hy );
        states[i].v_xt = (float)( states[0].v_xt + rn[2] * VELOCITY_DISTURB );
        states[i].v_yt = (float)( states[0].v_yt + rn[3] * VELOCITY_DISTURB );
        states[i].Hxt = (int)( states[0].Hxt + rn[4] * SCALE_DISTURB );
        states[i].Hyt = (int)( states[0].Hyt + rn[5] * SCALE_DISTURB );
        states[i].at_dot = (float)( states[0].at_dot + rn[6] * SCALE_CHANGE_D );
        /* 权重统一为1/N,让每个粒子有相等的机会 */
        weights[i] = (float)(1.0/NParticle);
    }
    
    return( 1 );
}



/*
 计算归一化累计概率c'_i
 输入参数:
 float * weight:    为一个有N个权重(概率)的数组
 int N:             数组元素个数
 输出参数:
 float * cumulateWeight: 为一个有N+1个累计权重的数组,
 cumulateWeight[0] = 0;
 */
void NormalizeCumulatedWeight( float * weight, float * cumulateWeight, int N )
{
    int i;
    
    for ( i = 0; i < N+1; i++ )
        cumulateWeight[i] = 0;
    for ( i = 0; i < N; i++ )
        cumulateWeight[i+1] = cumulateWeight[i] + weight[i];
    for ( i = 0; i < N+1; i++ )
        cumulateWeight[i] = cumulateWeight[i]/ cumulateWeight[N];
    
    return;
}

/*
 折半查找,在数组NCumuWeight[N]中寻找一个最小的j,使得
 NCumuWeight[j] <=v
 float v:              一个给定的随机数
 float * NCumuWeight:  权重数组
 int N:                数组维数
 返回值:
 数组下标序号
 */
int BinearySearch( float v, float * NCumuWeight, int N )
{
    int l, r, m;
    
    l = 0; 	r = N-1;   /* extreme left and extreme right components' indexes */
    while ( r >= l)
    {
        m = (l+r)/2;
        if ( v >= NCumuWeight[m] && v < NCumuWeight[m+1] ) return( m );
        if ( v < NCumuWeight[m] ) r = m - 1;
        else l = m + 1;
    }
    return( 0 );
}

/*
 重新进行重要性采样
 输入参数:
 float * c:          对应样本权重数组pi(n)
 int N:              权重数组、重采样索引数组元素个数
 输出参数:
 int * ResampleIndex:重采样索引数组
 */
void 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;
}

/*
 样本选择,从N个输入样本中根据权重重新挑选出N个
 输入参数:
 SPACESTATE * state:     原始样本集合(共N个)
 float * weight:         N个原始样本对应的权重
 int N:                  样本个数
 输出参数:
 SPACESTATE * state:     更新过的样本集
 */
void 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;
}

/*
 传播:根据系统状态方程求取状态预测量
 状态方程为: S(t) = A S(t-1) + W(t-1)
 W(t-1)为高斯噪声
 输入参数:
 SPACESTATE * state:      待求的状态量数组
 int N:                   待求状态个数
 输出参数:
 SPACESTATE * state:      更新后的预测状态量数组
 */
void Propagate( SPACESTATE * state, int N)
{
    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);
        cvCircle(pTrackImg,cvPoint(state[i].xt,state[i].yt),3, CV_RGB(0,255,0),-1);
    }
    return;
}

/*
 观测,根据状态集合St中的每一个采样,观测直方图,然后
 更新估计量,获得新的权重概率
 输入参数:
 SPACESTATE * state:      状态量数组
 int N:                   状态量数组维数
 unsigned char * image:   图像数据,按从左至右,从上至下的顺序扫描,
 颜色排列次序:RGB, RGB, ...
 int W, H:                图像的宽和高
 float * ObjectHist:      目标直方图
 int hbins:               目标直方图条数
 输出参数:
 float * weight:          更新后的权重
 */
void 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;
}

/*
 估计,根据权重,估计一个状态量作为跟踪输出
 输入参数:
 SPACESTATE * state:      状态量数组
 float * weight:          对应权重
 int N:                   状态量数组维数
 输出参数:
 SPACESTATE * EstState:   估计出的状态量
 */
void 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 + 0.5 );
    EstState.Hyt = (int)(Hyt/weight_sum + 0.5 );
    EstState.v_xt = v_xt/weight_sum;
    EstState.v_yt = v_yt/weight_sum;
    EstState.xt = (int)(xt/weight_sum + 0.5 );
    EstState.yt = (int)(yt/weight_sum + 0.5 );
    
    return;
}


/************************************************************
 模型更新
 输入参数:
 SPACESTATE EstState:   状态量的估计值
 float * TargetHist:    目标直方图
 int bins:              直方图条数
 float PiT:             阈值(权重阈值)
 unsigned char * img:   图像数据,RGB形式
 int W, H:              图像宽高
 输出:
 float * TargetHist:    更新的目标直方图
 ************************************************************/
# define ALPHA_COEFFICIENT      0.2     /* 目标模型更新权重取0.1-0.3 */

int ModelUpdate( SPACESTATE EstState, float * TargetHist, int bins, float PiT,
                unsigned char * img, int W, int H )
{
    float * EstHist, Bha, Pi_E;
    int i, rvalue = -1;
    
    EstHist = new float [bins];
    
    /* (1)在估计值处计算目标直方图 */
    CalcuColorHistogram( EstState.xt, EstState.yt, EstState.Hxt,
                        EstState.Hyt, img, W, H, EstHist, bins );
    /* (2)计算Bhattacharyya系数 */
    Bha  = CalcuBhattacharyya( EstHist, TargetHist, bins );
    /* (3)计算概率权重 */
    Pi_E = CalcuWeightedPi( Bha );
    
    if ( Pi_E > PiT )
    {
        for ( i = 0; i < bins; i++ )
        {
            TargetHist[i] = (float)((1.0 - ALPHA_COEFFICIENT) * TargetHist[i]
                                    + ALPHA_COEFFICIENT * EstHist[i]);
        }
        rvalue = 1;
    }
    
    delete EstHist;
    
    return( rvalue );
}

/*
 系统清除
 */
void ClearAll()
{
    if ( ModelHist != NULL ) delete [] ModelHist;
    if ( states != NULL ) delete [] states;
    if ( weights != NULL ) delete [] weights;
    
    return;
}

/**********************************************************************
 基于彩色直方图的粒子滤波算法总流程
 输入参数:
 unsigned char * img: 图像数据,RGB形式
 int W, H:            图像宽高
 输出参数:
 int &xc, &yc:        找到的图像目标区域中心坐标
 int &Wx_h, &Hy_h:    找到的目标的半宽高
 float &max_weight:   最大权重值
 返回值:
 成功1,否则-1
 
 基于彩色直方图的粒子滤波跟踪算法的完整使用方法为:
 (1)读取彩色视频中的1帧,并确定初始区域,以此获得该区域的中心点、
 目标的半高、宽,和图像数组(RGB形式)、图像高宽参数。
 采用初始化函数进行初始化
 int Initialize( int x0, int y0, int Wx, int Hy,
 unsigned char * img, int W, int H )
 (2)循环调用下面函数,直到N帧图像结束
 int ColorParticleTracking( unsigned char * image, int W, int H,
 int & xc, int & yc, int & Wx_h, int & Hy_h )
 每次调用的输出为:目标中心坐标和目标的半高宽
 如果函数返回值<0,则表明目标丢失。
 (3)清除系统各个变量,结束跟踪
 void ClearAll()
 
 **********************************************************************/
int ColorParticleTracking( unsigned char * image, int W, int H,
                          int & xc, int & yc, int & Wx_h, int & Hy_h,
                          float & max_weight)
{
    SPACESTATE EState;
    int i;
    /* 选择:选择样本,并进行重采样 */
    ReSelect( states, weights, NParticle );
    /* 传播:采样状态方程,对状态变量进行预测 */
    Propagate( states, NParticle);
    /* 观测:对状态量进行更新 */
    Observe( states, weights, NParticle, image, W, H,
            ModelHist, nbin );
    /* 估计:对状态量进行估计,提取位置量 */
    Estimation( states, weights, NParticle, EState );
    xc = EState.xt;
    yc = EState.yt;
    Wx_h = EState.Hxt;
    Hy_h = EState.Hyt;
    /* 模型更新 */
    ModelUpdate( EState, ModelHist, nbin, Pi_Thres,	image, W, H );
    
    /* 计算最大权重值 */
    max_weight = weights[0];
    for ( i = 1; i < NParticle; i++ )
        max_weight = max_weight < weights[i] ? weights[i] : max_weight;
    /* 进行合法性检验,不合法返回-1 */
    if ( xc < 0 || yc < 0 || xc >= W || yc >= H ||
        Wx_h <= 0 || Hy_h <= 0 ) return( -1 );
    else
        return( 1 );
}



//把iplimage 转到img 数组中,BGR->RGB
void IplToImge(IplImage* src, int w,int h)
{
    int i,j;
    for ( j = 0; j < h; j++ ) // 转成正向图像
        for ( i = 0; i < w; i++ )
        {
            img[ ( j*w+i )*3 ] = R(src,i,j);
            img[ ( j*w+i )*3+1 ] = G(src,i,j);
            img[ ( j*w+i )*3+2 ] = B(src,i,j);
        }
}
void mouseHandler(int event, int x, int y, int flags, void* param)//在这里要注意到要再次调用cvShowImage,才能显示方框
{
    CvMemStorage* storage = cvCreateMemStorage(0);
    CvSeq * contours;
    IplImage* pFrontImg1 = 0;
    int centerX,centerY;
    int delt = 10;
    pFrontImg1=cvCloneImage(pFrontImg);//这里也要注意到如果在 cvShowImage("foreground",pFrontImg1)中用pFrontImg产效果,得重新定义并复制
    switch(event){
        case CV_EVENT_LBUTTONDOWN:
            //printf("laskjfkoasfl\n");
            //寻找轮廓
            if(pause)
            {
                cvFindContours(pFrontImg,storage,&contours,sizeof(CvContour),CV_RETR_EXTERNAL,
                               CV_CHAIN_APPROX_SIMPLE);
                
                //在原场景中绘制目标轮廓的外接矩形
                for (;contours;contours = contours->h_next)
                {
                    CvRect r = ((CvContour*)contours)->rect;
                    if(x>r.x&&x<(r.x+r.width)&&y>r.y&&r.y<(r.y+r.height))
                    {
                        if (r.height*r.width>CONTOUR_MIN_AREA && r.height*r.width<CONTOUR_MAX_AREA)
                        {
                            centerX = r.x+r.width/2;//得到目标中心点
                            centerY = r.y+r.height/2;
                            WidIn = r.width/2;//得到目标半宽与半高
                            HeiIn = r.height/2;
                            xin = centerX;
                            yin = centerY;
                            cvRectangle(pFrontImg1,cvPoint(r.x,r.y),cvPoint(r.x+r.width,r.y+r.height),cvScalar(255,255,255),2,8,0);
                            //Initial_MeanShift_tracker(centerX,centerY,WidIn,HeiIn,img,Wid,Hei,1./delt);  //初始化跟踪变量
                            /* 初始化跟踪器 */
                            Initialize( centerX, centerY, WidIn, HeiIn, img, Wid, Hei );
                            track = true;//进行跟踪
                            cvShowImage("foreground",pFrontImg1);
                            return;
                            
                        }
                    }
                    
                }
            }
            
            break;
            
        case CV_EVENT_LBUTTONUP:
            printf("Left button up\n");
            break;
    }
}
//void on_mouse(int event, int x, int y, int flags, void *param)
//{
//	if(!image)
//		return ;
//	if(image->origin)
//	{
//		image->origin = 0;
//		y = image->height - y;
//	}
//	if(selecting) //正在选择物体
//	{
//		selection.x = MIN(x,origin.x);
//		selection.y = MIN(y,origin.y);
//		selection.width = selection.x + CV_IABS(x - origin.x);
//		selection.height = selection.y + CV_IABS(y - origin.y);
//
//		selection.x = MAX(selection.x ,0);
//		selection.y = MAX(selection.y,0);
//		selection.width = MIN(selection.width,image->width);
//		selection.height = MIN(selection.height,image->height);
//		selection.width -= selection.x;
//		selection.height -= selection.y;
//	}
//	switch(event)
//	{
//	case CV_EVENT_LBUTTONDOWN:
//		origin = cvPoint(x,y);
//		selection = cvRect(x,y,0,0);
//		selecting = 1;
//		break;
//	case CV_EVENT_LBUTTONUP:
//		selecting = 0;
//		if(selection.width >0 && selection.height >0)
//			selected = 1;
//		break;
//	}
//}

int main(int argc, char *argv[])
{
    int FrameNum=0;  //帧号
    int k=0;
    CvCapture *capture = cvCreateFileCapture("/Users/liupeng/Desktop/my/opencvLearn/opencvLearn/video2.mp4");
    char res1[20],res2[20];
    //CvCapture *capture = cvCreateFileCapture("test1.avi");
    //CvCapture *capture = cvCreateFileCapture("camera1_mov.avi");
    IplImage* frame[Num]; //用来存放图像
    int i,j;
    uchar key = false;      //用来设置暂停
    float rho_v;//表示相似度
    float max_weight;
    
    int sum=0;    //用来存放两图像帧差后的值
    for (i=0;i<Num;i++)
    {
        frame[i]=NULL;
    }
    
    IplImage *curFrameGray=NULL;
    IplImage *frameGray=NULL;
    
    CvMat *Mat_D,*Mat_F;   //动态矩阵与帧差后矩阵
    int row ,col;
    cvNamedWindow("video",1);
    
    cvNamedWindow("background",1);
    cvNamedWindow("foreground",1);
    cvNamedWindow("tracking",1);
    cvSetMouseCallback("tracking",mouseHandler,0);//响应鼠标
    
    while (capture)
    {
        curframe=cvQueryFrame(capture); //抓取一帧
        if(FrameNum<Num)
        {
            if(FrameNum==0)//第一帧时初始化过程
            {
                curFrameGray=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
                frameGray=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
                pBackImg=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
                pFrontImg=cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,1);
                pTrackImg = cvCreateImage(cvGetSize(curframe),IPL_DEPTH_8U,3);
                
                cvSetZero(pFrontImg);
                cvCvtColor(curframe,pBackImg,CV_RGB2GRAY);
                
                row=curframe->height;
                col=curframe->width;
                Mat_D=cvCreateMat(row,col,CV_32FC1);
                cvSetZero(Mat_D);
                Mat_F=cvCreateMat(row,col,CV_32FC1);
                cvSetZero(Mat_F);
                Wid = curframe->width;
                Hei = curframe->height;
                img = new unsigned char [Wid * Hei * 3];
            }
            frame[k]=cvCloneImage(curframe);  //把前num帧存入到图像数组
            pTrackImg = cvCloneImage(curframe);
        }
        else
        {
            k=FrameNum%Num;
            pTrackImg = cvCloneImage(curframe);
            IplToImge(curframe,Wid,Hei);
            cvCvtColor(curframe,curFrameGray,CV_RGB2GRAY);
            cvCvtColor(frame[k],frameGray,CV_RGB2GRAY);	
            for(i=0;i<curframe->height;i++)
                for(j=0;j<curframe->width;j++)
                {
                    sum=S(curFrameGray,j,i)-S(frameGray,j,i);
                    sum=sum<0 ? -sum : sum;
                    if(sum>T)   //文献中公式(1)
                    {
                        CV_MAT_ELEM(*Mat_F,float,i,j)=1;
                    }
                    else 
                    {
                        CV_MAT_ELEM(*Mat_F,float,i,j)=0;
                    }
                    
                    if(CV_MAT_ELEM(*Mat_F,float,i,j)!=0)//文献中公式(2)
                        CV_MAT_ELEM(*Mat_D,float,i,j)=Re;
                    else{
                        if(CV_MAT_ELEM(*Mat_D,float,i,j)!=0)
                            CV_MAT_ELEM(*Mat_D,float,i,j)=CV_MAT_ELEM(*Mat_D,float,i,j)-1;
                    }
                    if(CV_MAT_ELEM(*Mat_D,float,i,j)==0.0)
                    {
                        //文献中公式(3)
                        S(pBackImg,j,i)=(uchar)((1-ai)*S(pBackImg,j,i)+ai*S(curFrameGray,j,i));
                    }
                    sum=S(curFrameGray,j,i)-S(pBackImg,j,i);//背景差分法
                    sum=sum<0 ? -sum : sum;
                    if(sum>40)
                    {
                        S(pFrontImg,j,i)=255;
                    }
                    else 
                        S(pFrontImg,j,i)=0;
                    
                }
            frame[k]=cvCloneImage(curframe); 
        }
        FrameNum++;	
        k++;
        cout<<FrameNum<<endl;
        
        //进行形态学滤波,去噪
        cvDilate(pFrontImg, pFrontImg, 0, 2);
        cvErode(pFrontImg, pFrontImg, 0, 3);
        cvDilate(pFrontImg, pFrontImg, 0, 1);
        if(track)
        {
            /* 跟踪一帧 */
            rho_v = ColorParticleTracking( img, Wid, Hei, xout, yout, WidOut, HeiOut, max_weight);
            /* 画框: 新位置为蓝框 */
            if ( rho_v > 0 && max_weight > 0.0001 )  /* 判断是否目标丢失 */
            {
                cvRectangle(pFrontImg,cvPoint(xout - WidOut,yout - HeiOut),cvPoint(xout+WidOut,yout+HeiOut),cvScalar(255,255,255),2,8,0);
                cvRectangle(pTrackImg,cvPoint(xout - WidOut,yout - HeiOut),cvPoint(xout+WidOut,yout+HeiOut),cvScalar(255,255,255),2,8,0);
                xin = xout; yin = yout;
                WidIn = WidOut; HeiIn = HeiOut;
                /*draw_rectangle( pBuffer, Width, Height, xo, Height-yo-1, wo, ho, 0x00ff0000, 2 );
                 xb = xo; yb = yo;
                 wb = wo; hb = ho;*/
            }
        }
        
        cvShowImage("video",curframe);
        cvShowImage("foreground",pFrontImg);
        cvShowImage("background",pBackImg);
        cvShowImage("tracking",pTrackImg);
        /*sprintf(res1,"fore%d.jpg",FrameNum);
         cvSaveImage(res1,pFrontImg);
         sprintf(res2,"ground%d.jpg",FrameNum);
         cvSaveImage(res2,pBackImg);*/
        cvSetMouseCallback("foreground",mouseHandler,0);//响应鼠标
        key = cvWaitKey(1);
        if(key == 'p') pause = true;
        while(pause)
            if(cvWaitKey(0)=='p')
                pause = false;		
        
    }
    cvReleaseImage(&curFrameGray);
    cvReleaseImage(&frameGray);
    cvReleaseImage(&pBackImg);
    cvReleaseImage(&pFrontImg);
    cvDestroyAllWindows();
    //	Clear_MeanShift_tracker();
    ClearAll();
    

}






代码实现:

运行方式:按P停止,在前景窗口鼠标点击目标,会自动生成外接矩形,再次按P,对该选定目标进行跟踪。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
// 新版本写在下面文件中:
#include <opencv2/nonfree/features2d.hpp>
//#include "opencv2/features2d/features2d.hpp"
#include<opencv2/legacy/legacy.hpp>

using namespace std;
using namespace cv;


// 以下这些参数对结果影响很大,而且也会根据视频内容,会对结果有很大的影响
const int PARTICLE_NUM = 25;	// 粒子个数
// 粒子放入的相关区域
const double A1 = 2.0;
const double A2 = -1.0;
const double B0 = 1.0;
// 高斯随机数sigma参数
const double SIGMA_X = 1.0;
const double SIGMA_Y = 0.5;
const double SIGMA_SCALE = 0.001;

// 粒子结构体
typedef struct particle {
    double x;			// 当前x坐标
    double y;			// 当前y坐标
    double scale;		// 窗口比例系数
    double xPre;			// x坐标预测位置
    double yPre;			// y坐标预测位置
    double scalePre;		// 窗口预测比例系数
    double xOri;			// 原始x坐标
    double yOri;			// 原始y坐标
    // 	int width;			// 原始区域宽度
    // 	int height;			// 原始区域高度
    Rect rect;			// 原始区域大小
    MatND hist;			// 粒子区域的特征直方图
    double weight;		// 该粒子的权重
} PARTICLE;

Mat hsv;	// hsv色彩空间的输入图像
Mat roiImage;	// 目标区域
MatND roiHist;	// 目标区域直方图
Mat img;	// 输出的目标图像
PARTICLE particles[PARTICLE_NUM];	// 粒子

int nFrameNum = 0;

bool bSelectObject = false;	// 区域选择标志
bool bTracking = false;		// 开始跟踪标志
Point origin;	// 鼠标按下时的点位置
Rect selection;// 感兴趣的区域大小

// 直方图相关参数,特征的选取也会对结果影响巨大
// Quantize the hue to 30 levels
// and the saturation to 32 levels
// value to 10 levels
int hbins = 180, sbins = 256, vbin = 10;
int histSize[] = {hbins, sbins, vbin};
// hue varies from 0 to 179, see cvtColor
float hranges[] = { 0, 180 };
// saturation varies from 0 (black-gray-white) to 255 (pure spectrum color)
float sranges[] = { 0, 256 };
// value varies from 0 (black-gray-white) to 255 (pure spectrum color)
float vranges[] = { 0, 256 };
const float* ranges[] = {hranges, sranges, vranges};
// we compute the histogram from the 0-th and 1-st channels
int channels[] = {0, 1, 2};

// 鼠标响应函数,得到选择的区域,保存在selection
void onMouse(int event, int x, int y, int, void*)
{
    if( bSelectObject )
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);
        
        selection &= Rect(0, 0, img.cols, img.rows);
    }
    
    switch (event)
    {
        case CV_EVENT_LBUTTONDOWN:
            origin = Point(x,y);
            selection = Rect(x,y,0,0);
            bSelectObject = true;
            bTracking = false;
            break;
        case CV_EVENT_LBUTTONUP:
            bSelectObject = false;
            bTracking = true;
            nFrameNum = 0;
            break;
    }
}

// 快速排序算法排序函数
int particle_cmp(const void* p1,const void* p2)
{
    PARTICLE* _p1 = (PARTICLE*)p1;
    PARTICLE* _p2 = (PARTICLE*)p2;
    
    if(_p1->weight < _p2->weight)
        return 1;	//按照权重降序排序
    if(_p1->weight > _p2->weight)
        return -1;
    return 0;
}

int main(int argc, char *argv[])
{
    int delay = 10;	// 控制播放速度
    char c;	// 键值
    
    VideoCapture captRefrnc("/Users/liupeng/Desktop/my/opencvLearn/opencvLearn/video2.mp4");	// 视频文件
    
    if ( !captRefrnc.isOpened())
    {
        return -1;
    }
    
    //VideoCapture captRefrnc(0);   //打开摄像头
    //if (!captRefrnc.isOpened())   // isOpened函数用来检测VideoCapture类是否打开成功
    //{
    //    return -1;
    //}
    
    // Windows
    // 	const char* WIN_SRC = "Source";
    const char* WIN_RESULT = "Result";
    // 	namedWindow(WIN_SRC, CV_WINDOW_AUTOSIZE );
    namedWindow(WIN_RESULT, CV_WINDOW_AUTOSIZE);
    // 鼠标响应函数
    setMouseCallback(WIN_RESULT, onMouse, 0);
    
    Mat frame;	//视频的每一帧图像
    
    bool paused = false;
    PARTICLE * pParticles = particles;
    //	PARTICLE * pParticles = new PARTICLE[sizeof(PARTICLE) * PARTICLE_NUM];
    
    while(true) //Show the image captured in the window and repeat
    {
        if(!paused)
        {
            captRefrnc >> frame;
            if(frame.empty())
                break;
        }
        
        frame.copyTo(img);	// 接下来的操作都是对src的
        
        // 选择目标后进行跟踪
        if (bTracking == true)
        {
            if(!paused)
            {
                nFrameNum++;
                cvtColor(img, hsv, CV_BGR2HSV);
                Mat roiImage(hsv, selection);	// 目标区域
                
                if (nFrameNum == 1)	//选择目标后的第一帧需要初始化
                {
                    // step 1: 提取目标区域特征
                    calcHist(&roiImage, 1, channels, Mat(), roiHist, 3, histSize, ranges);
                    normalize(roiHist, roiHist);	// 归一化L2
                    
                    // step 2: 初始化particle
                    pParticles = particles;
                    
                    for (int i=0; i<PARTICLE_NUM; i++)
                    {
                        pParticles->x = selection.x + 0.5 * selection.width;
                        pParticles->y = selection.y + 0.5 * selection.height;
                        pParticles->xPre = pParticles->x;
                        pParticles->yPre = pParticles->y;
                        pParticles->xOri = pParticles->x;
                        pParticles->yOri = pParticles->y;
                        pParticles->rect = selection;
                        pParticles->scale = 1.0;
                        pParticles->scalePre = 1.0;
                        pParticles->hist = roiHist;
                        pParticles->weight = 0;
                        pParticles++;
                    }
                }
                else
                {
                    pParticles = particles;
                    
                    RNG rng;
                    for (int i=0; i<PARTICLE_NUM; i++)
                    {
                        // step 3: 求particle的transition
                        double x, y, s;
                        
                        pParticles->xPre = pParticles->x;
                        pParticles->yPre = pParticles->y;
                        pParticles->scalePre = pParticles->scale;
                        
                        x = A1 * (pParticles->x - pParticles->xOri) + A2 * (pParticles->xPre - pParticles->xOri) +
                        B0 * rng.gaussian(SIGMA_X) + pParticles->xOri;
                        pParticles->x = std::max(0.0, std::min(x, img.cols-1.0));
                        
                        
                        y = A1 * (pParticles->y - pParticles->yOri) + A2 * (pParticles->yPre - pParticles->yOri) +
                        B0 * rng.gaussian(SIGMA_Y) + pParticles->yOri;
                        pParticles->y = std::max(0.0, std::min(y, img.rows-1.0));
                        
                        s = A1 * (pParticles->scale - 1.0) + A2 * (pParticles->scalePre - 1.0) +
                        B0 * rng.gaussian(SIGMA_SCALE) + 1.0;
                        pParticles->scale = std::max(0.1, std::min(s, 3.0));
                        // rect参数有待考证
                        pParticles->rect.x = std::max(0, std::min(cvRound(pParticles->x - 0.5 * pParticles->rect.width * pParticles->scale), img.cols-1));		// 0 <= x <= img.rows-1
                        pParticles->rect.y = std::max(0, std::min(cvRound(pParticles->y - 0.5 * pParticles->rect.height * pParticles->scale), img.rows-1));	// 0 <= y <= img.cols-1
                        pParticles->rect.width = std::min(cvRound(pParticles->rect.width * pParticles->scale), img.cols - pParticles->rect.x);
                        pParticles->rect.height = std::min(cvRound(pParticles->rect.height * pParticles->scale), img.rows - pParticles->rect.y);
                        // Ori参数不改变
                        
                        // step 4: 求particle区域的特征直方图
                        Mat imgParticle(img, pParticles->rect);
                        calcHist(&imgParticle, 1, channels, Mat(), pParticles->hist, 3, histSize, ranges);
                        normalize(pParticles->hist, pParticles->hist);	// 归一化L2
                        
                        // step 5: 特征的比对,更新particle权重
                        pParticles->weight = compareHist(roiHist, pParticles->hist, CV_COMP_INTERSECT);
                        
                        pParticles++;
                    }
                    
                    // step 6: 归一化粒子权重
                    double sum = 0.0;
                    int i;
                    
                    pParticles = particles;
                    
                    for(i=0; i<PARTICLE_NUM; i++)
                    {
                        sum += pParticles->weight;
                        pParticles++;
                    }
                    pParticles = particles;
                    
                    for(i=0; i<PARTICLE_NUM; i++)
                    {
                        pParticles->weight /= sum;
                        pParticles++;
                    }
                    
                    // step 7: resample根据粒子的权重的后验概率分布重新采样
                    pParticles = particles;
                   
                    // 					PARTICLE* newParticles = new PARTICLE[sizeof(PARTICLE) * PARTICLE_NUM];
                    PARTICLE newParticles[PARTICLE_NUM];
                    int np, k = 0;
                    
                    qsort(pParticles, PARTICLE_NUM, sizeof(PARTICLE), &particle_cmp);
                    for(int i=0; i<PARTICLE_NUM; i++)
                    {
                        np = cvRound(particles[i].weight * PARTICLE_NUM);
                        for(int j=0; j<np; j++)
                        {
                            newParticles[k++] = particles[i];
                            if(k == PARTICLE_NUM)
                                goto EXITOUT;
                        }
                    }
                    while(k < PARTICLE_NUM)
                    {
                        newParticles[k++] = particles[0];
                    }
                    
                EXITOUT:
                    for (int i=0; i<PARTICLE_NUM; i++)
                    {
                        particles[i] = newParticles[i];
                    }
                    
                }// end else
                
                
                pParticles = particles;
                qsort(pParticles, PARTICLE_NUM, sizeof(PARTICLE), &particle_cmp);
                
                // step 8: 计算粒子的期望,作为跟踪结果
                Rect_<double> rectTrackingTemp(0.0, 0.0, 0.0, 0.0);
                pParticles = particles;
                for (int i=0; i<PARTICLE_NUM; i++)
                {
                    rectTrackingTemp.x += pParticles->rect.x * pParticles->weight;
                    rectTrackingTemp.y += pParticles->rect.y * pParticles->weight;
                    rectTrackingTemp.width += pParticles->rect.width * pParticles->weight;
                    rectTrackingTemp.height += pParticles->rect.height * pParticles->weight;
                    pParticles++;
                }
                // Rect rectTracking(rectTrackingTemp);	// 跟踪结果
                Rect rectTracking;
                rectTracking.x = rectTrackingTemp.x;
                rectTracking.y = rectTrackingTemp.y;
                rectTracking.height = rectTrackingTemp.height;
                rectTracking.width =rectTrackingTemp.width;
                
                
                // 显示各粒子的运动
                for (int i=0; i<PARTICLE_NUM; i++)
                {
                    rectangle(img, particles[i].rect, Scalar(255,0,0));
                }
                // 显示跟踪结果
                rectangle(img, rectTracking, Scalar(0,0,255), 3);
                
            }
        }// end Tracking
        
        // 		imshow(WIN_SRC, frame);
        imshow(WIN_RESULT, img);
        
        c = (char)waitKey(delay);
        if( c == 27 )
            break;
        switch(c)
        {
            case 'p':
                paused = !paused;
                break;
            default:
                ;
        }
    }// end while
}





  • 2
    点赞
  • 11
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 2
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

MachineLP

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值