在我的最前面的几篇博文中,我详细介绍过卡尔曼滤波器KF,扩展卡尔曼滤波器EKF和无损(无迹)卡尔曼滤波器UKF. 它们都是基于贝叶斯(Bayes)概率思想的滤波器.熟悉ROS的朋友都知道,ROS系统有一个BFL库(The Bayesian Filtering Library),我们熟悉的KF,EKF,UKF,ParticlesFilter等都是BFL支持的滤波器.关于贝叶斯概率和它下面一系列滤波器我打算下有空总结一下(有时间的话).
对于粒子滤波器的理论,书籍和网上都很容易找到,本文不做介绍,重点放在实践上. 你主要知道粒子滤波器的重要的几个步骤就可以:
1. 初始化粒子,包括总个数,位置和权值等等;
2. 按照一定的算法扩散粒子,让粒子去寻找目标;
3. 根据各个粒子的情况,计算其权值,并归一化权值;
4. 根据各个粒子的归一化权值,获取最优估计;
5. 粒子重采样,基本原则是: 权值低于阈值的舍弃; 根据权值大小衍生新粒子;确保粒子总数不变.
本文介绍实现一个基于粒子滤波器,使用openCV HSV直方图作为图像特征的目标物体跟踪程序(仅供演示).
下面结合粒子滤波器通用算法流程,介绍下本例的实现思路(后面上效果图和代码):
1. 粒子初始化;
1>.使用openCV计算目标区域尺寸ROI和HSV直方图特征,我们称为Target;
2>.初始化所有粒子,总个数为N,因为我们一开始就目标明确,所以初始化的粒子全部在Target所在位置;
3>.每个粒子具有和Target一样尺寸的ROI(位置不同);
4>.全部粒子的初始权值值0,或1/N均可;
2. 更新每个粒子的位置,比对图像特征,更新粒子权值;
1>.利用一个称为二阶动态回归的方法,更新所有粒子的位置,更新的位置具有从粒子当前位置向外径向扩散的趋势;
2>.获取每个粒子的ROI对应的图像的HSV直方图特征,与Target的HSV直方图特征进行"巴氏距离"比对;
3>.根据巴氏距离的比对结果,归一化和更新每个粒子的权值;
3. 状态估计输出:
1>. 对所有粒子按权值降序排序,这样粒子序列中第一个粒子就是最优位置估计粒子,其后的4个也可以做为较优估计作为参考;
4. 按照粒子权值对所有粒子进行重采样:
1>. 对于权值过小的粒子直接抛弃,这样总的粒子数就减小为N-n;
2>. 按照一定的比例,对排序过的权值有效的粒子进行自我复制,原则就是排名越靠前,自我复制的粒子越多;
3>. 确保新生成的粒子集总数仍为N;
4>.重复第2,3,4步.
实际运行时,由于HSV直方图特征的问题,识别跟踪容易受目标物体背景,物体缩放,旋转等因素干扰, 这是直方图的弊端,更换为其它鲁棒性更高图像特征算法(比如:KCF物体跟踪算法采用的傅里叶变换)效果会更好. 如果选取目标时,选取目标物体局部区域,不选取任何背景,效果也能接受. 当然这都不是重点,我们的重点是实践粒子滤波器.
下面上效果图:
运行时,视频上出现100个(默认)粒子,以绿色小方点表示,粒子位置表示当前位置;按照粒子匹配对权值,取匹配对最佳的前5个粒子, 最佳No.1用红色矩形框标注,后面第2-第4使用绿色框标注.
使用方法: 运行后再"原始视频"窗口,用鼠标左键按下并拖拉选择目标物体,然后开始基于粒子的物体跟踪.
选取目标时带蓝色天空背景, 背景时蓝天时,效果还可以:
飞入白云,HSV直方图特征受背景色干扰,无法匹配目标:
飞出白云,粒子们给力, 又重新锁定飞机:
选取目标时带蓝色天空背景, 背景是白云且目标缩小时,粒子无法再锁定飞机,粒子整体呈随机满屏搜索状态:
选取目标局部区域并不带背景, 粒子跟踪效果可以:
选取目标局部区域并不带背景, 粒子跟踪效果可以:
选取目标局部区域并不带背景, 粒子跟踪效果可以:
选取目标局部区域并不带背景, 粒子跟踪效果可以:
因为图像特征采用的是HSV直方图,所以受图像色调,色相影响很大, 只加了一层蒙版层,HSV直方图特征就无法正常匹配, 我的粒子门又处于散乱随机搜索状态:
最后,代码:
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <ctime>
#include <fstream>
#include <iostream>
#include <assert.h>
#include <thread>
#include<queue>
using namespace cv;
using namespace std;
#define PARTICLE_COUNT (100) //粒子数目
#define MATCH_THRESHOLD (0.7) //HSV直方图对比巴氏距离阈值
class Particle
{
public:
int orix, oriy; //原始粒子坐标
int x, y; //当前粒子的坐标
double scale; //当前粒子窗口的尺寸
int prex, prey; //上一帧粒子的坐标
double prescale; //上一帧粒子窗口的尺寸
Rect rectRoi; //当前粒子矩形窗口
double weight; //当前粒子权值
int T;
};
class ParticleTrack {
private:
bool trackPaused = true;
bool foundMatches = false; //本次或上次检测,是否匹配到目标
//****有关粒子窗口变化用到的相关变量,用于更新当前粒子的位置****/
int A1 = 2;
int A2 = -1;
int B0 = 1;
double sigmax = 1.0;
double sigmay = 0.5;
double sigmas = 0.001;
int times = 0;//计算次数
//直方图
MatND targetHist;
Rect targetRectRoi;
vector<Particle> newParticle;//定义一个新的粒子数组
vector<Particle> particles; // 粒子参数
private:
//对粒子权重值的降序排列
static bool compareFunc(Particle p1, Particle p2)
{
return p1.weight > p2.weight;
}
//计算指定image的直方图
static MatND calculateHistogram(Mat imgRoi) {
Mat hsvImage;
// 【1】将图像由BGR色彩空间转换到 HSV色彩空间
cvtColor(imgRoi, hsvImage, COLOR_BGR2HSV);
//【2】初始化计算直方图需要的实参
// 对hue通道使用30个bin,对saturatoin通道使用32个bin
int h_bins = 50; int s_bins = 60;
int histSize[] = { h_bins, s_bins };
// hue的取值范围从0到256, saturation取值范围从0到180
float h_ranges[] = { 0, 256 };
float s_ranges[] = { 0, 180 };
const float* ranges[] = { h_ranges, s_ranges };
// 使用第0和第1通道
int channels[] = { 0, 1 };
// 【3】创建储存直方图的 MatND 类的实例:
MatND baseHist;
// 【4】计算基准图像，的HSV直方图:
calcHist(&hsvImage, 1, channels, Mat(), baseHist, 2, histSize, ranges, true, false);
normalize(baseHist, baseHist, 0, 1, NORM_MINMAX, -1, Mat());
//获取巴氏距离方法.值越小,越相似
//double base_base = compareHist(baseHist, baseHist, CV_COMP_BHATTACHARYYA);//获取巴氏距离
return baseHist;
}
public:
void pause() {
trackPaused = true;
}
void resume() {
trackPaused = false;
}
bool isPaused() {
return trackPaused;
}
void setTarget(Mat imgRoi, Rect &rectRoi)
{
times = 0;
targetRectRoi = rectRoi;
targetHist = calculateHistogram(imgRoi);
particles.clear();
// 目标粒子初始化
for (int k = 0; k < PARTICLE_COUNT; k++) //对于每个粒子
{
Particle particle;
particle.x = cvRound(rectRoi.x + 0.5*rectRoi.width);//当前粒子的x坐标
particle.y = cvRound(rectRoi.y + 0.5*rectRoi.height);//当前粒子的y坐标
//粒子的原始坐标为选定矩形框(即目标)的中心
particle.orix = particle.x;
particle.oriy = particle.y;
//当前粒子窗口的尺寸
particle.scale = 1;//初始化为1，然后后面粒子到搜索的时候才通过计算更新
//更新上一帧粒子的坐标
particle.prex = particle.x;
particle.prey = particle.y;
//上一帧粒子窗口的尺寸
particle.prescale = 1;
//当前粒子矩形窗口
particle.rectRoi = rectRoi;
//当前粒子权值
particle.weight = 0;//权重初始为0
particle.T = 0;
particles.push_back(particle);
}
}
Mat doParticleTracking(Mat imgFrame)
{
int xpre, ypre;
double prescale, scale;
int x, y;
double sum = 0.0;
RNG rng; //随机数产生器
bool lastFoundMatches = foundMatches;
foundMatches = false;
//动态更新每个粒子的位置,并根据和目标的距离,计算粒子的权重W
for (int k = 0; k < PARTICLE_COUNT; k++)
{
if (lastFoundMatches) { //上次至少一个检测到目标
//当前粒子的坐标
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, imgFrame.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, imgFrame.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;
}
else {
particles.at(k).x = rand() % imgFrame.cols;//当前粒子的x坐标 随机
particles.at(k).y = rand() % imgFrame.rows;//当前粒子的y坐标 随机
//粒子的原始坐标为选定矩形框(即目标)的中心
particles.at(k).orix = particles.at(k).x;
particles.at(k).oriy = particles.at(k).y;
//当前粒子窗口的尺寸
particles.at(k).scale = 1;//初始化为1，然后后面粒子到搜索的时候才通过计算更新
//更新上一帧粒子的坐标
particles.at(k).prex = particles.at(k).x;
particles.at(k).prey = particles.at(k).y;
//上一帧粒子窗口的尺寸
particles.at(k).prescale = 1;
//当前粒子矩形窗口
particles.at(k).rectRoi.x = particles.at(k).x;
particles.at(k).rectRoi.y = particles.at(k).y;
particles.at(k).rectRoi.width = targetRectRoi.width;
particles.at(k).rectRoi.height = targetRectRoi.height;
//当前粒子权值
particles.at(k).weight = 0;//权重初始为0
particles.at(k).T = 0;
}
/*计算更新得到矩形框数据*/
particles.at(k).rectRoi.x = max(0, min(cvRound(particles.at(k).x - 0.5*particles.at(k).scale*particles.at(k).rectRoi.width), imgFrame.cols));
particles.at(k).rectRoi.y = max(0, min(cvRound(particles.at(k).y - 0.5*particles.at(k).scale*particles.at(k).rectRoi.height), imgFrame.rows));
particles.at(k).rectRoi.width = min(cvRound(particles.at(k).rectRoi.width), imgFrame.cols - particles.at(k).rectRoi.x);
particles.at(k).rectRoi.height = min(cvRound(particles.at(k).rectRoi.height), imgFrame.rows - particles.at(k).rectRoi.y);
MatND particleHist = calculateHistogram(Mat(imgFrame, particles.at(k).rectRoi));
double weight = compareHist(targetHist, particleHist, CV_COMP_BHATTACHARYYA);//获取巴氏距离,值越小,约相似
if (weight <= MATCH_THRESHOLD)
foundMatches = true;
particles.at(k).weight = 1 - weight; //这个必须有!
/*粒子权重累加*/
sum += particles.at(k).weight;
}
Rect rectDot;
rectDot.width = 5;
rectDot.height = 5;
if (!foundMatches) { //本次没有匹配到(均>巴氏距离阈值),画出随机粒子,直接返回
for (int k = 0; k < PARTICLE_COUNT; k++) {
//画出随机粒子
rectDot.x = particles.at(k).rectRoi.x + particles.at(k).rectRoi.width / 2;
rectDot.y = particles.at(k).rectRoi.y + particles.at(k).rectRoi.height / 2;
rectangle(imgFrame, rectDot, Scalar(0, 255, 0), 1, 8, 0);//显示跟踪结果，框出
}
return imgFrame;
}
// 赋值每个粒子权重
int countT = 0;
int countParticle = 0;
//归一化权值
for (int k = 0; k < PARTICLE_COUNT; k++)
{
particles.at(k).weight /= sum;
particles.at(k).T = cvRound(particles.at(k).weight*PARTICLE_COUNT);
if (particles.at(k).T > 0) {
countT += particles.at(k).T;
countParticle++;
}
//前面都已经识别完毕,标注出画粒子
rectDot.x = particles.at(k).rectRoi.x + particles.at(k).rectRoi.width / 2;
rectDot.y = particles.at(k).rectRoi.y + particles.at(k).rectRoi.height / 2;
rectangle(imgFrame, rectDot, Scalar(0, 255, 0), 1, 8, 0);//显示跟踪结果，框出
}
// 对归一化后权重排序
sort(particles.begin(), particles.end(), ParticleTrack::compareFunc);
//取前1/4个粒子作为跟踪结果,计算其最大权重目标的期望位置
Rect rectTracking; //初始化一个Rect作为跟踪的临时
double weight_temp = 0.0;
for (int k = 0; k < PARTICLE_COUNT / 4; k++)
{
weight_temp += particles.at(k).weight;
}
for (int k = 0; k < PARTICLE_COUNT / 4; k++)
{
particles.at(k).weight /= weight_temp;
}
//更新检测框,显示匹配最佳的第一个(红色)和匹配优秀的前5个(绿色,包含第一个已红色)
for (int k = 0; k < 5; k++)
{
rectTracking.x = particles.at(k).rectRoi.x;
rectTracking.y = particles.at(k).rectRoi.y;
rectTracking.width = particles.at(k).rectRoi.width;
rectTracking.height = particles.at(k).rectRoi.height;
if (k == 0)
rectangle(imgFrame, rectTracking, Scalar(0, 0, 255), 1, 8, 0);//显示跟踪结果，红框
else
rectangle(imgFrame, rectTracking, Scalar(0, 255, 0), 1, 8, 0);//显示跟踪结果，红框
}
bool flag = false; // 完成粒子赋值,结束标志
newParticle.clear();
//重采样，根据粒子权重 决定重采样粒子多少
for (int k = 0; k < PARTICLE_COUNT; k++)
{
if (flag) // 完成粒子赋值，跳出循环
{
break;
}
if (particles.at(k).T > 0)
{
int generateCount = (int)((particles.at(k).T*1.0 / countParticle)*PARTICLE_COUNT);
for (int i = 0; i < generateCount; i++) // 权重越大，该点赋值数越多
{
newParticle.push_back(particles.at(k));
if (newParticle.size() >= PARTICLE_COUNT)
{
flag = true;
break;
}
}
}
}
//没有采样满PARTICLE_COUNT个,复制并填充最大权值的信息
while (newParticle.size() < PARTICLE_COUNT)
{
newParticle.push_back(particles.at(0));//复制相关性最大的权值的样本填满剩余粒子空间
}
//更新粒子
particles.clear();
// 将粒子点替换为更新后的粒子点
for (int k = 0; k < PARTICLE_COUNT; k++)
{
//重新计算粒子检测框尺寸, 避免萎缩过小
if (newParticle.at(k).rectRoi.width < targetRectRoi.width)
newParticle.at(k).rectRoi.width = imgFrame.cols - newParticle.at(k).x;
if (newParticle.at(k).rectRoi.height < targetRectRoi.height)
newParticle.at(k).rectRoi.height = imgFrame.rows - newParticle.at(k).y;
particles.push_back(newParticle.at(k));
}
cout << times++ << endl; // 输出检测帧数
return imgFrame;
}
};
bool leftButtonDownFlag = false; //左键单击后的标志位
bool leftButtonUpFlag = false; //左键单击后松开的标志位
Point Point_s; //目标矩形框选取起点
Point Point_move; //鼠标按下移动过程中,矩形框移动的当前点
Point Point_e; //目标矩形框选取终点,鼠标左键弹起来的终点
bool selectTargetCompleted = false;
bool saveImage = false;
int saveImageNameIndex = 0;
void onMouse(int event, int x, int y, int flags, void *param)
{
if (event == CV_EVENT_RBUTTONDOWN)
{
saveImage = true;
}
if (event == CV_EVENT_LBUTTONDOWN)
{
selectTargetCompleted = false;
leftButtonDownFlag = true; //标志位
leftButtonUpFlag = false;
Point_move = Point(x, y); //设置左键按下点的矩形起点
Point_s = Point_move;
}
else if (event == CV_EVENT_MOUSEMOVE && leftButtonDownFlag)
{
Point_move = Point(x, y);
}
else if (event == CV_EVENT_LBUTTONUP && leftButtonDownFlag)
{
leftButtonDownFlag = false;
Point_move = Point(x, y);
Point_e = Point_move;
leftButtonUpFlag = true;
selectTargetCompleted = true;
}
}
queue<Mat> imageFrameQueue;
queue<Mat> resultFrameQueue;
bool threadWorking = true;
//粒子跟踪线程
void particleTrackThread(ParticleTrack *pParticleTrack) {
if (pParticleTrack == NULL)
return;
while (threadWorking) {
if (!pParticleTrack->isPaused() && selectTargetCompleted && imageFrameQueue.size() >= 1) {
Mat imageFrame = imageFrameQueue.front();
imageFrameQueue.pop();
blur(imageFrame, imageFrame, Size(2, 2));//先对原图进行均值滤波处理
imageFrame = pParticleTrack->doParticleTracking(imageFrame);
resultFrameQueue.push(imageFrame); //将i压入队列的尾部
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
//这里是入口函数,若果独立运行,请修改成main()
//int doParticleFilter_main(/*int argc, char **argv*/)
int main(int argc, char **argv)
{
Mat frame,imgROI;
Rect rectROI;
//打开摄像头或者特定视频
VideoCapture videoCapture;
videoCapture.open(0); //打开摄像头
//videoCapture.open("saiche.mp4");//打开视频文件
//读入视频是否为空
if (!videoCapture.isOpened())
{
return -1;
}
namedWindow("原始视频", 1);
namedWindow("目标跟踪视频", 1);
namedWindow("选中的ROI区域", 1);
setMouseCallback("原始视频", onMouse, 0);//鼠标回调函数，响应鼠标以选择跟踪区域
ParticleTrack particleTrack_1;
threadWorking = true;
std::thread workingThread(particleTrackThread, &particleTrack_1);//启动粒子跟踪线程
while (1)
{
videoCapture >> frame;
if (frame.empty())
{
threadWorking = false;
workingThread.join();
return -1;
}
if (leftButtonDownFlag) // 绘制截取目标窗口
{
particleTrack_1.pause();
rectROI.x = Point_s.x;
rectROI.y = Point_s.y;
rectROI.width = Point_move.x - Point_s.x;
rectROI.height = Point_move.y - Point_s.y;
rectangle(frame, rectROI, Scalar(0, 255, 0), 3, 8, 0);
cout << "rectROI: "<<rectROI << endl;
}
if (selectTargetCompleted && leftButtonUpFlag)
{
leftButtonUpFlag = false;
rectROI.x = Point_s.x;
rectROI.y = Point_s.y;
rectROI.width = Point_e.x - Point_s.x;
rectROI.height = Point_e.y - Point_s.y;
imgROI = Mat(frame, rectROI); //目标图像
imgROI = imgROI.clone();//复制一份出来
imshow("选中的ROI区域", imgROI);
particleTrack_1.setTarget(imgROI, rectROI);
particleTrack_1.resume();
}
imshow("原始视频", frame);
if (selectTargetCompleted)
{
if (imageFrameQueue.size() > 1)
imageFrameQueue.pop();
imageFrameQueue.push(frame.clone()); //将新image压入队列的尾部
}
if (resultFrameQueue.size() >= 1) {
Mat resultFrame = resultFrameQueue.front();
resultFrameQueue.pop();
imshow("目标跟踪视频", resultFrame);
if (saveImage) {
saveImage = false;
string name = "image_" + to_string(saveImageNameIndex)+ ".png";
saveImageNameIndex++;
imwrite(name, resultFrame);
}
}
if (cv::waitKey(50) >= 0)
break;
}
threadWorking = false;
workingThread.join();
return 0;
}最后,可以改进的地方:
1. 上面代码是一个单目标跟踪,有兴趣的可以尝试把代码中particleTrackThread线程和消息队列放入ParticleTrack类内部,并对目标选取做修改,把上面代码改造成多目标跟踪. 太多不敢说,跟踪的2-3个还是可以的;
2.基于直方图的特征匹配实在太粗暴,有兴趣的可以试试边缘检测+傅里叶描述子的方法, 傅里叶描述子具有平移、旋转、尺度不变性, 可以大大提高匹配的抗干扰性.
关于使用边缘提取+傅里叶算子做目标匹配, 多年前做智能机顶盒时,曾经为公司写过一个直播电视频道识别(台标识别)功能,效果那是相当不错.
3.有兴趣的可以参考KCF (Kernelized correlation filter核相关过滤器)追踪方法.跟踪效果和跟踪速度上都有十分亮眼的表现.
觉得对你有帮助的话,麻烦给的赞,谢谢!
878
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
