粒子滤波算法理解及实现
粒子滤波算法是一种非线性的滤波方法。其大致思路如下（这里以图像目标（人）跟踪为例）：1、首先在整个图像中随机初始化一些粒子点，并对每个粒子点分配权值2、在视频中框出待跟踪目标3、更新权值，增加靠近框出的目标粒子权值4、根据状态转移矩阵和测量数据，对粒子权重，对粒子进行重采样粒子滤波示过程示意图初始化图像粒子点和权重框出待跟踪目标更新权重，其中权重较小的直接舍弃，权值较...
·
文档下载链接：https://download.csdn.net/download/OEMT_301/12069538
https://download.csdn.net/download/OEMT_301/12104738
粒子滤波算法是一种非线性的滤波方法。
其大致思路如下（这里以图像目标（人）跟踪为例）：
1、 首先在整个图像中随机初始化一些粒子点，并对每个粒子点分配权值
2、 在视频中框出待跟踪目标
3、 更新权值，增加靠近框出的目标粒子权值
4、 根据状态转移矩阵和测量数据，对粒子权重，对粒子进行重采样
粒子滤波示过程示意图
初始化图像粒子点和权重
框出待跟踪目标
更新权重，其中权重较小的直接舍弃，权值较大的粒子点进行复制，权值越大复制的粒子点越多。
经过几次权值更新，可以使得大多数粒子点处于目标框附近
根据状态转移矩阵获取新粒子点坐标（箭头相当于状态转移矩阵）
A、经过目标移动后，理想状态为如下图
B、但实际上存在一些噪声，预测结果与实际结果有偏差
因此继续重采样，利用测量结果对其进行重采样，便可得到较为准确的跟踪效果
以上为粒子滤波过程示意图
重采样
MATLAB重采样代码示意
function w_new=resample_particles(w)
w_new=w;
Neff=1/sum(w.*w);
N=length(w);
if Neff<75 %75为预先设定阈值
for i = 1 : N
u = rand;
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + w(j);
if qtempsum >= u
w_new(i)=w(j);
break;
end
end
end
end
一维SIR粒子滤波实例
%% initialize the variables
x = 0.1; % initial actual state
x_N = 1; % 系统过程噪声的协方差 (由于是一维的，这里就是方差)
x_R = 1; % 测量的协方差
T = 75; % 共进行75次
N = 100; % 粒子数，越大效果越好，计算量也越大
%initilize our initial, prior particle distribution as a gaussian around
%the true initial value
V = 2; %初始分布的方差
x_P = []; % 粒子
% 用一个高斯分布随机的产生初始的粒子
for i = 1:N
x_P(i) = x + sqrt(V) * randn;
end
z_out = [x^2 / 20 + sqrt(x_R) * randn]; %实际测量值
x_out = [x]; %the actual output vector for measurement values.
x_est = [x]; % time by time output of the particle filters estimate
x_est_out = [x_est]; % the vector of particle filter estimates.
for t = 1:T
x = 0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
z = x^2/20 + sqrt(x_R)*randn;
for i = 1:N
%从先验p(x(k)|x(k-1))中采样
x_P_update(i) = 0.5*x_P(i) + 25*x_P(i)/(1 + x_P(i)^2) + 8*cos(1.2*(t-1)) + sqrt(x_N)*randn;
%计算采样粒子的值，为后面根据似然去计算权重做铺垫
z_update(i) = x_P_update(i)^2/20;
%对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式
P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));
end
% 归一化.
P_w = P_w./sum(P_w);
%% Resampling
%这里没有用博客里之前说的histc函数，不过目的和效果是一样的
for i = 1 : N
x_P(i) = x_P_update(find(rand <= cumsum(P_w),1)); % 粒子权重大的将多得到后代
end % find( ,1) 返回第一个 符合前面条件的数的 下标
%状态估计，重采样以后，每个粒子的权重都变成了1/N
x_est = mean(x_P);
% Save data in arrays for later plotting
x_out = [x_out x];
z_out = [z_out z];
x_est_out = [x_est_out x_est];
end
t = 0:T;
figure(1);
clf
plot(t, x_out, '.-b', t, x_est_out, '-.r','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('time step'); ylabel('flight position');
legend('True flight position', 'Particle filter estimate');
图像粒子跟踪实例
main.m
% Parameters
F_update = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1]; % 状态转移矩阵
Npop_particles = 4000;
Xstd_rgb = 50; % 方差
Xstd_pos = 25; % 位置倍率
Xstd_vec = 5; % 速度倍率
Xrgb_trgt = [255; 0; 0]; % 检测颜色
% Loading Movie
vr = VideoReader('Person.wmv');
Npix_resolution = [vr.Width vr.Height];
Nfrm_movie = floor(vr.Duration * vr.FrameRate);
% Object Tracking by Particle Filter
X = create_particles(Npix_resolution, Npop_particles); % 粒子初始化，在画面中产生均匀分布的随机粒子
for k = 1:Nfrm_movie
% Getting Image
Y_k = read(vr, k);
% Forecasting
%通过状态模型预测 这里采用的是在上一时刻基础上叠加噪声
X = update_particles(F_update, Xstd_pos, Xstd_vec, X);
% Calculating Log Likelihood
L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X(1:2, :), Y_k);
% Resampling
X = resample_particles(X, L);
% Showing Image
show_particles(X, Y_k);
% show_state_estimated(X, Y_k);
end
create_particles.m
function X = create_particles(Npix_resolution, Npop_particles)
X1 = randi(Npix_resolution(2), 1, Npop_particles); % 产生一个 1 x Npop_particles 的行向量，各元素值为 1：Npix_resolution(2)之间的产生的均匀分布的随机整数
X2 = randi(Npix_resolution(1), 1, Npop_particles);
X3 = zeros(2, Npop_particles);
X = [X1; X2; X3];
update_particles.m
function X = update_particles(F_update, Xstd_pos, Xstd_vec, X)
% X 所有粒子组成的矩阵
% X(1:2, :) 各粒子在画面中的位置
N = size(X, 2);
X = F_update * X; % 状态转移矩阵
X(1:2,:) = X(1:2,:) + Xstd_pos * randn(2, N);
X(3:4,:) = X(3:4,:) + Xstd_vec * randn(2, N);
calc_log_likelihood.m
function L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X, Y) %#codegen
Npix_h = size(Y, 1);
Npix_w = size(Y, 2);
N = size(X,2);
L = zeros(1,N);
Y = permute(Y, [3 1 2]); %重新安排矩阵
% 高斯函数
A = -log(sqrt(2 * pi) * Xstd_rgb);
B = - 0.5 / (Xstd_rgb.^2);
X = round(X);
for k = 1:N
% m,n 粒子k在画面中的位置
m = X(1,k);
n = X(2,k);
% 越界判断
I = (m >= 1 & m <= Npix_h);
J = (n >= 1 & n <= Npix_w);
if I && J
C = double(Y(:, m, n));
D = C - Xrgb_trgt;
D2 = D' * D; % 欧氏距离
L(k) = A + B * D2; %高斯似然 D2 越小 L越大 注意B为负数
else
L(k) = -Inf;
end
end
resample_particles.m
function X = resample_particles(X, L_log)
% Calculating Cumulative Distribution
L = exp(L_log - max(L_log));
Q = L / sum(L, 2); % a = sum(L,2) a中元素为各行向量累加值
R = cumsum(Q, 2); % 权值累加
% Generating Random Numbers
N = size(X, 2);
T = rand(1, N); % 随机阈值
% Resampling
X_temp = zeros(size(X));
%这里没有用博客里之前说的histc函数，不过目的和效果是一样的
for i = 1 : N
X_temp(:,i) = X(:,find(T(i) <= R,1)); % 粒子权重大的将多得到后代
end % find( ,1) 返回第一个 符合前面条件的数的 下标
X = X_temp;
% [~, I] = histc(T, R);
% X = X(:, I + 1);
C++代码：
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <ctime>
#include <fstream>
#include <iostream>
#include <assert.h>
using namespace cv;
using namespace std;
//****************************定义粒子数目**********************************//
#define PARTICLE_NUMBER 300
#define HIST_SIZE 16
//*******************************定义粒子结构体类型************************//
typedef struct particle//关于typedef struct和struct见下文补充
{
int orix, oriy; //原始粒子坐标
int x, y; //当前粒子的坐标
double scale; //当前粒子窗口的尺寸
int prex, prey; //上一帧粒子的坐标
double prescale; //上一帧粒子窗口的尺寸
Rect rect; //当前粒子矩形窗口
double weight; //当前粒子权值
}PARTICLE;
bool leftButtonDownFlag=false; //左键单击后的标志位
bool leftButtonUpFlag=false; //左键单击后松开的标志位
Point Point_s; //矩形框起点
Point Point_e; //矩形框鼠标左键弹起来的终点
Point processPoint; //矩形框移动的终点
bool tracking = false;
//****有关粒子窗口变化用到的相关变量****// 目标运行预测值
int A1 = 2;
int A2 = -1;
int B0 = 1;
double sigmax = 1.0;
double sigmay = 0.5;
double sigmas = 0.001;
double *m_hist, *hist;
int p_num = 0;
vector<PARTICLE> newParticle;//定义一个新的粒子数组
/****粒子权重值的降序排列****/
// 排序法则，其中：< 升序 >降序
bool comparison(PARTICLE p1,PARTICLE p2)
{
return p1.weight > p2.weight ;
}
void onMouse( int event, int x, int y, int flags, void *param )
{
if(event==CV_EVENT_LBUTTONDOWN)
{
tracking = false;
leftButtonDownFlag = true; //标志位
leftButtonUpFlag = false;
processPoint=Point(x,y); //设置左键按下点的矩形起点
Point_s=processPoint;
}
else if(event == CV_EVENT_MOUSEMOVE && leftButtonDownFlag)
{
processPoint=Point(x,y);
}
else if(event==CV_EVENT_LBUTTONUP && leftButtonDownFlag)
{
leftButtonDownFlag=false;
processPoint=Point(x,y);
Point_e=processPoint;
leftButtonUpFlag = true;
tracking = true;
}
}
void init_target(Mat mould, vector<PARTICLE> &particles, Rect &rect)
{
int q_r, q_g, q_b, q_temp;
particles.clear();
//初始化权值矩阵和目标直方图
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
m_hist[i] = 0.0;
}
//计算目标权值直方
for (int i = 0;i < mould.rows; i++)
{
for (int j = 0;j < mould.cols; j++)
{
//rgb颜色空间量化为16*16*16 bins
q_r = mould.at<Vec3b>(i, j)[2]/255/HIST_SIZE;
q_g = mould.at<Vec3b>(i, j)[1]/255/HIST_SIZE;
q_b = mould.at<Vec3b>(i, j)[0]/255/HIST_SIZE;
q_temp = q_r * HIST_SIZE * HIST_SIZE + q_g * HIST_SIZE + q_b;
m_hist[q_temp]++; // 颜色权重直方图
}
}
// 归一化
double m_hist_sum = 0.0;
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
m_hist_sum += pow(m_hist[i], 2);
}
m_hist_sum = sqrt(m_hist_sum);
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
m_hist[i] = m_hist[i]/m_hist_sum;
}
// 目标粒子初始化
PARTICLE pParticle;
for (int k=0; k<PARTICLE_NUMBER; k++) //对于每个粒子
{
pParticle.x = cvRound(rect.x + 0.5*rect.width);//当前粒子的x坐标
pParticle.y = cvRound(rect.y + 0.5*rect.height);//当前粒子的y坐标
//粒子的原始坐标为选定矩形框(即目标)的中心
pParticle.orix = pParticle.x;
pParticle.oriy = pParticle.y;
//当前粒子窗口的尺寸
pParticle.scale = 1;//初始化为1，然后后面粒子到搜索的时候才通过计算更新
//更新上一帧粒子的坐标
pParticle.prex = pParticle.x;
pParticle.prey = pParticle.y;
//上一帧粒子窗口的尺寸
pParticle.prescale = 1;
//当前粒子矩形窗口
pParticle.rect = rect;
//当前粒子权值
pParticle.weight = 0;//权重初始为0
particles.push_back(pParticle);
}
}
void My_Tracking(Mat img, vector<PARTICLE> &particles)
{
int xpre, ypre;
double prescale, scale;
int x, y;
double sum = 0.0;
RNG rng; //随机数产生器
int q_r, q_g, q_b, q_temp;
/*计算粒子区域的直方图*/
//初始化目标直方图
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
hist[i] = 0.0;
}
for (int k=0; k<PARTICLE_NUMBER; k++)
{
//当前粒子的坐标
xpre = particles.at(k).x;
ypre = particles.at(k).y;
//当前粒子窗口的尺寸
prescale = particles.at(k).scale;
/*更新跟踪矩形框中心，即粒子中心*///使用二阶动态回归来自动更新粒子状态
x = cvRound(A1*(particles.at(k).x - particles.at(k).orix) + A2*(particles.at(k).prex - particles.at(k).orix) +
B0*rng.gaussian(sigmax) + particles.at(k).orix);
particles.at(k).x = max(0, min(x, img.cols - 1));
y = cvRound(A1*(particles.at(k).y - particles.at(k).oriy) + A2*(particles.at(k).prey - particles.at(k).oriy) +
B0*rng.gaussian(sigmay) + particles.at(k).oriy);
particles.at(k).y = max(0, min(y, img.rows - 1));
scale = A1*(particles.at(k).scale - 1) + A2*(particles.at(k).prescale - 1) + B0*(rng.gaussian(sigmas)) + 1.0;
particles.at(k).scale = max(1.0, min(scale, 3.0));//此处参数设置存疑
particles.at(k).prex = xpre;
particles.at(k).prey = ypre;
particles.at(k).prescale = prescale;
/*计算更新得到矩形框数据*/
particles.at(k).rect.x = max(0, min(cvRound(particles.at(k).x - 0.5*particles.at(k).scale*particles.at(k).rect.width), img.cols));
particles.at(k).rect.y = max(0, min(cvRound(particles.at(k).y - 0.5*particles.at(k).scale*particles.at(k).rect.height), img.rows));
particles.at(k).rect.width = min(cvRound(particles.at(k).rect.width), img.cols - particles.at(k).rect.x);
particles.at(k).rect.height = min(cvRound(particles.at(k).rect.height), img.rows - particles.at(k).rect.y);
//计算粒子权值直方图
for (int i = particles.at(k).rect.y; i < particles.at(k).rect.y + particles.at(k).rect.height; i++)
{
for (int j = particles.at(k).rect.x; j < particles.at(k).rect.x + particles.at(k).rect.width; j++)
{
//rgb颜色空间量化为16*16*16 bins
q_r = img.at<Vec3b>(i, j)[2]/255/HIST_SIZE;
q_g = img.at<Vec3b>(i, j)[1]/255/HIST_SIZE;
q_b = img.at<Vec3b>(i, j)[0]/255/HIST_SIZE;
q_temp = q_r * HIST_SIZE * HIST_SIZE + q_g * HIST_SIZE + q_b;
hist[q_temp]++; // 颜色权重直方图
}
}
// 直方图归一化
double hist_sum = 0.0;
double sim_sum = 0.0;
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
hist_sum += pow(hist[i], 2);
}
hist_sum = sqrt(hist_sum);
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
hist[i] = hist[i]/hist_sum;
if(m_hist[i] > 0.0 && hist[i] > 0.0)
{
sim_sum += sqrt(m_hist[i]*hist[i]); // 计算巴氏距离
}
}
particles.at(k).weight = sim_sum;
/*粒子权重累加*/
sum += particles.at(k).weight;
}
// 赋值每个粒子权重
for (int k=0; k<PARTICLE_NUMBER; k++)
{
particles.at(k).weight /= sum;
}
sort(particles.begin(), particles.end(), comparison); // 权重排序
//*********************重采样，根据粒子权重重采样********************//
int T = 0; // 阈值，只要T个粒子
bool flag = false; // 是否对每个粒子重新赋值跳出标志位
newParticle.clear();
for (int k = 0;k < PARTICLE_NUMBER;k++)
{
if(flag) // 完成粒子赋值，跳出循环
{
break;
}
T = cvRound(particles.at(k).weight*PARTICLE_NUMBER); //将权重较弱的粒子淘汰掉，保留权重在阈值以上的
if(T > 0)
{
for (int i = 0;i < T;i++) // 权重越大，该点赋值数越多
{
newParticle.push_back(particles.at(k));
if (newParticle.size() >= PARTICLE_NUMBER)
{
flag = true;
break;
}
}
}
else // 没有可以满足条件的权值，跳出循环
{
break;
}
}
if(!flag) // 点未全部赋值，将剩下的用最大值进行赋值
{
while (newParticle.size() < PARTICLE_NUMBER)
{
newParticle.push_back(particles.at(0));//复制大的权值的样本填满空间
}
}
// 将粒子点替换为更新后的粒子点
for (int k = 0;k < PARTICLE_NUMBER;k++)
{
particles.at(k) = newParticle.at(k);
}
//***********计算最大权重目标的期望位置，采用权值最大的1/4个粒子数作为跟踪结果************//
Rect rectTracking; //初始化一个Rect作为跟踪的临时
double weight_temp = 0.0;
for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
{
weight_temp += particles.at(k).weight;
}
for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
{
particles.at(k).weight /= weight_temp;
}
// 更新检测框
for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
{
rectTracking.x += particles.at(k).rect.x*particles.at(k).weight;
rectTracking.y += particles.at(k).rect.y*particles.at(k).weight;
rectTracking.width += particles.at(k).rect.width*particles.at(k).weight;
rectTracking.height += particles.at(k).rect.height*particles.at(k).weight;
}
rectangle(img, rectTracking, Scalar(0, 255, 0), 3, 8, 0);//显示跟踪结果，框出
}
int main()
{
Mat img_mould, frame, mould;
Rect rect;
vector<PARTICLE> particles; // 粒子参数
//打开摄像头或者特定视频
VideoCapture cap;
cap.open(0);//或cap.open("文件名")
//读入视频是否为空
if (!cap.isOpened())
{
return -1;
}
namedWindow("输出视频", 1);
setMouseCallback("输出视频", onMouse, 0);//鼠标回调函数，响应鼠标以选择跟踪区域
m_hist = (double *)malloc(sizeof(double)*HIST_SIZE * HIST_SIZE * HIST_SIZE); // 目标直方图
hist = (double *)malloc(sizeof(double)*HIST_SIZE * HIST_SIZE * HIST_SIZE); // 目标直方图
while(1)
{
cap >> frame;
if (frame.empty())
{
return -1;
}
blur(frame, frame, Size(2, 2));//先对原图进行均值滤波处理
if(tracking && leftButtonUpFlag)
{
leftButtonUpFlag = false;
rect.x = Point_s.x;
rect.y = Point_s.y;
rect.width = Point_e.x - Point_s.x;
rect.height = Point_e.y - Point_s.y;
img_mould = frame.clone();
mould = Mat(img_mould, rect); //目标图像
init_target(mould, particles, rect);
p_num = 0;
}
if(leftButtonDownFlag) // 绘制截取目标窗口
{
rect.x = Point_s.x;
rect.y = Point_s.y;
rect.width = processPoint.x - Point_s.x;
rect.height = processPoint.y - Point_s.y;
rectangle(frame, rect, Scalar(0, 255, 0), 3, 8, 0);
}
if(tracking)
{
My_Tracking(frame, particles);
cout << p_num++ << endl; // 输出检测帧数
}
imshow("输出视频", frame);
waitKey(1);
}
return 0;
}
注：C++代码是直接调动摄像头，效率比较低，识别准确率也有待提高，以后有时间了再优化。
自我感觉自己理解可能有误，请谨慎参考
参考：
http://blog.csdn.net/heyijia0327/article/details/40899819
https://blog.csdn.net/u011624019/article/details/80559397
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
35
0- 0
已为社区贡献1条内容
所有评论(0)