视频目标跟踪算法

在室内移动机器人的应用中，有时候采用粒子滤波算法进行自定位（听同学讲的，我也不懂啦）。
非常专业、非常学院派的说法是——粒子滤波是通过非参数化的蒙特卡洛模拟方法来实现递推贝叶斯滤波。作为一个机械专业的小白，我花了大量精力理解这句话，好不容易混到一知半解的程度，可惜隔两个天又混沌了。
不过，从代码出发的话，好理解多了。csdn也有一些帮助理解的文章。

/*****************************************************************************************************

     关于粒子滤波，一开始我试图看书看文献，失败了。这次重新捡起。
我对粒子滤波的理解：
    1. 如何让目标寻找更有方向性？
        （1） 每次都随机、均匀的选取一些区域，和目标区域比较，选择最相似的。
        （2） 上述（1）方法还是慢，那就不随机的选取区域，就在上次求解出来的目标附近来随机选取一些区域，选择最相似的。
    2. 上述1（2）中的方法比较靠谱，看看粒子滤波是怎么具体实现的。
        （1）人工手动选择目标区域（当然牛逼的可以采用自动识别的方式），并基于HSV或RGB颜色空间，建立目标区域的特征。
        （2）撒粒子，也就是选择一些区域了，后面要用这些粒子和目标区域比较呢。 但怎么撒呢？ 就是利用状态转移方程预测一下
        粒子的位置，以预测的位置为中心，按高斯分布来撒。
                状态转移方程又是什么呢？ 具体在本文中，就是用前两帧的粒子位置，来预测当前的粒子位置。
        （3）粒子撒完了，就和目标比对呗。根据比对的相似性，给每个粒子赋一个权重。
        （4）粒子也撒了，权重也算了，那目标到底会在哪呢？ 可以认定权重最大的粒子为跟踪目标的位置，
        也可以认定所有粒子的加权平均（期望）为目标位置。
        （5）over。

原文链接 http://www.cnblogs.com/tornadomeet/archive/2012/03/18/2404817.html
我对原文代码的主要修改：
    （1）从可读性的角度，做了些修改和注释。
    （2）删除了qsort排序算法
    （3）原文显示跟踪结果的代码，位置不对，应该在重采样之前。

        注：本算法的跟踪效果无遮挡等情况下较好。
****************************************************************************************************/

#include   
#include         // Basic OpenCV structures (cv::Mat, Scalar)
#include   // OpenCV window I/O
#include
using namespace cv;


Rect select_rect;           //鼠标选定的目标区域
bool tracking = false;  //为true时，跟踪算法开始计算（其值由鼠标相应事件决定）
bool select_show = false;   //为true后，显示目标选定框（其值由鼠标相应事件决定）

Mat frame, hsv_mat; 
int frame_count = 0;//选择矩形区域完后的帧计数

/****hsv空间用到的变量****/
int hist_size[] = { 16, 16, 16 };   // 三通道的bins
float hrange[] = { 0, 180};
float srange[] = { 0, 255};
float vrange[] = { 0, 255};
const float *ranges[] = { hrange, srange, vrange };
int channels[] = { 0, 1, 2 };       //计算直方图函数的 传入参数

/****有关粒子窗口变化用到的相关变量****/
int A1 = 2, A2 = -1, B0 = 1;    // 状态转移的参数（二阶自回归运动模型）
double sigmax = 1.0, sigmay = 0.5, sigmas = 0.001; //产生高斯随机噪声的参数

/****定义粒子结构体****/
struct Particle
{
    int orix, oriy;//原始粒子坐标
    int x, y;//当前粒子的坐标
    double scale;//当前粒子窗口的尺寸
    int prex, prey;//上一帧粒子的坐标
    double prescale;//上一帧粒子窗口的尺寸
    Rect rect;//当前粒子矩形窗口
    Mat hist;//当前粒子窗口直方图特征
    double weight;//当前粒子权值
};
const int particle_num = 100;       //粒子数目
Particle particles[particle_num];       //粒子群

/******鼠标相应函数*******/
void onMouse(int event, int x, int y, int, void*)
{
    static Point origin;     // 原文当作全局变量，这里定义为局部静态变量，可读性好些
    if (event == CV_EVENT_LBUTTONDOWN)
    {
        tracking = false;
        select_show = true;
        frame_count = 0;//还没开始选择，或者重新开始选择，计数为0
        origin = Point(x, y);//保存下来单击是捕捉到的点
        select_rect = Rect(x, y, 0, 0);
    }
    if (event == CV_EVENT_MOUSEMOVE)        //鼠标拖动时，实时计算矩形框
    {
        select_rect.x = MIN(origin.x, x);
        select_rect.y = MIN(origin.y, y);
        select_rect.width = abs(x - origin.x);//算矩形宽度和高度
        select_rect.height = abs(y - origin.y);
        select_rect &= Rect(0, 0, frame.cols, frame.rows);//&求并集，保证所选矩形框在视频显示区域之内
    }
    if (event == CV_EVENT_LBUTTONUP)
    {
        tracking = true;
        select_show = false;
    }
}

/****求解权重最大粒子的索引（原文采用快速排序，慢且没必要）。若以期望为跟踪目标，改写为求期望函数****/
size_t findMaxIndex(Particle* p, const int num)
{
    size_t index = 0;
    double maxWeight = 0.0;
    for (int i = 0; i < num; i++)
    {
        if (p->weight>maxWeight)
        {
            maxWeight = p->weight;
            index = i;
        }
    }
    return index;
}

void main()
{
    //导入视频文件
    VideoCapture video("soccer.avi");   
    if (!video.isOpened())      return;
    //读取一帧图像
    video >> frame;
    if (frame.empty())      return;
    //建立视频图像窗口
    namedWindow("视频窗口", 1);
    //鼠标响应事件
    setMouseCallback("视频窗口", onMouse, 0);

    Mat target_img, track_img;  //目标图像矩阵，粒子跟踪的图像矩阵
    Mat target_hist, track_hist;    //目标图像直方图，粒子跟踪的直方图

    while (true)
    {
        //读取一帧图像
        video >> frame;
        if (frame.empty())      return;

        //将BGR空间转换为hsv空间，因为创建frame时，默认是BGR颜色空间，不是RGB
        cvtColor(frame, hsv_mat, CV_BGR2HSV);

        if (tracking)
        {
            //第一帧
            if (0 == frame_count)
            {
                /****计算目标模板的直方图特征****/
                target_img = Mat(hsv_mat, select_rect);//在此之前先定义好target_img,然后这样赋值也行，要学会Mat的这个操作
                calcHist(&target_img, 1, channels, Mat(), target_hist, 3, hist_size, ranges);//计算颜色直方图
                normalize(target_hist, target_hist);//归一化

                /****初始化目标粒子****/
                for (int i = 0; i0.5*select_rect.width);//选定目标矩形框中心为初始粒子窗口中心
                    particles[i].y = cvRound(select_rect.y + 0.5*select_rect.height);
                    particles[i].orix = particles[i].x;//粒子的原始坐标为选定矩形框(即目标)的中心
                    particles[i].oriy = particles[i].y;
                    particles[i].prex = particles[i].x;//更新上一次的粒子位置
                    particles[i].prey = particles[i].y;
                    particles[i].rect = select_rect;
                    particles[i].prescale = 1;
                    particles[i].scale = 1;
                    particles[i].hist = target_hist;
                    // particles[i].weight = 0; // 全局变量默认的值是0
                }
            }
            //从第二帧开始就可以开始跟踪了
            else 
            {
                RNG rng;        //随机数产生器
                double sum = 0.0;   // 粒子权重和，后面权重归一化用
                // 计算每个粒子
                for (int i = 0; iint xtemp = particles[i].x;
                    int ytemp = particles[i].y;
                    double stemp = particles[i].scale;

                    /****更新粒子的矩形区域即粒子中心****/
                    // 接下来的x，y，scale的求解，是二阶自回归模型的状态转移方程（自回归就是，用某变量之前的值预测该变量，而不是预测其他变量）
                    // 说白了，就是利用前两帧（二阶）的粒子位置，预测当前的粒子位置，再加上一个高斯噪声。
                    // 加上高斯噪声的目的，类似于遗传算法中的基因突变，是粒子群更具有多样性。
                    int x = cvRound( A1*(particles[i].x - particles[i].orix) + A2*(particles[i].prex - particles[i].orix)
                        + particles[i].orix + B0*rng.gaussian(sigmax));
                    particles[i].x = max(0, min(x, frame.cols - 1)); // 实际就是x，避免过界

                    int y = cvRound(A1*(particles[i].y - particles[i].oriy) + A2*(particles[i].prey - particles[i].oriy) + particles[i].oriy + B0*rng.gaussian(sigmay));
                    particles[i].y = max(0, min(y, frame.rows - 1));

                    double scale = A1*(particles[i].scale - 1) + A2*(particles[i].prescale - 1) + B0*(rng.gaussian(sigmas)) + 1.0;
                    particles[i].scale = max(1.0, min(scale, 3.0));

                    particles[i].prex = xtemp;
                    particles[i].prey = ytemp;
                    particles[i].prescale = stemp;

                    particles[i].rect.x = max(0, min(particles[i].x - (int)(particles[i].scale*(particles[i].rect.width>>1)), frame.cols));     //位操作>>1实现除以2
                    particles[i].rect.y = max(0, min(particles[i].y - (int)(particles[i].scale*(particles[i].rect.height>>1)), frame.rows));
                    particles[i].rect.width = min(particles[i].rect.width, frame.cols - particles[i].rect.x);
                    particles[i].rect.height = min(particles[i].rect.height, frame.rows - particles[i].rect.y);

                    /****计算粒子区域的新的直方图特征****/
                    track_img = Mat(hsv_mat, particles[i].rect);
                    calcHist(&track_img, 1, channels, Mat(), track_hist, 3, hist_size, ranges); // 计算颜色直方图
                    normalize(track_hist, track_hist);  //归一化函数

                    //  particles[i].weight=compareHist(target_hist,track_hist,CV_COMP_INTERSECT);    // 权重更新
                    particles[i].weight = 1.0 - compareHist(target_hist, track_hist, CV_COMP_BHATTACHARYYA);//巴氏系数计算相似度（这里我不懂）

                    sum += particles[i].weight;//累加粒子权重
                }
                /****归一化粒子权重****/
                for (int i = 0; i/****权重最大的粒子作为跟踪结果，并显示（红色框）****/
                size_t max_index = findMaxIndex(particles, particle_num);   //求权重最大的粒子
                Rect rectTrackingTemp(particles[max_index].rect);
                Rect tracking_rect(rectTrackingTemp);
                rectangle(frame, tracking_rect, Scalar(0, 0, 255), 3, 8, 0);

                ///****显示各粒子运动结果（绿色圆点）****/
                for (int m = 0; m < particle_num; m++)
                    circle(frame, Point(particles[m].x, particles[m].y), 2, Scalar(255, 0, 0));


                /****根据粒子权重重采样粒子（原文重采样后再计算跟踪结果，是不对的）****/
                Particle newParticle[particle_num];
                int k = 0;
                for (int i = 0; iint num_p = cvRound(particles[i].weight * particle_num);    //作者的pParticle没有自增（本代码没有采用pParticle）
                    for (int j = 0; jif (k == particle_num)
                            goto EXITOUT;
                    }
                }
                while (kfor (int i = 0; i//end else

            /***总循环每循环一次，帧号加1***/
            frame_count++;
        }

        if (select_show)
            rectangle(frame, select_rect, Scalar(0, 0, 255), 3, 8, 0);//显示手动选择的矩形框

        imshow("视频窗口", frame);//显示视频图片到窗口
        waitKey(40);        //40ms，等待键盘输入，以控制播放速度
    }
}