一直都觉得粒子滤波是个挺牛的东西，每次试图看文献都被复杂的数学符号搞得看不下去。一个偶然的机会发现了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，对该选定目标进行跟踪。
代码语言：javascript复制//
// 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("laskjfkoasfln");
//寻找轮廓
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 upn");
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，对该选定目标进行跟踪。
代码语言：javascript复制#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
}
