目标跟踪学习笔记_3(particle filter初探2)

　　上次已经初步体验了下particle filter，直接用别人的代码，见我前面的博文http://www.cnblogs.com/tornadomeet/archive/2012/03/18/2404817.html

 　 一开始是内存出错，后面干脆自己学了下particle filter滤波的原理，把代码认真看了一遍，然后自己从头敲了遍代码，虽然运行时不再出现内存溢出等bug，但是没有跟踪效果。

　　这次的代码和上次一样，有跟踪效果，不过不理想。依旧是参照博主：http://blog.csdn.net/yang_xian521/article/details/6928131 的代码，但是算法稍微改了3点。

　　1.仔细阅读其代码发现其代码有粒子滤波原则性的错误，即物体的运动模型，作者本意是将运动模型近视为二阶自回归模型，但是程序中粒子的坐标计算过程中，前一次粒子坐标和当前粒子坐标是完全一样的。后面把这个bug改后。

　　2.最后跟踪物体的矩形采用权值最大粒子的矩形，而不是才用所以粒子的期望，经过试验发现采用权值最大的粒子的效果要好些。

　　3.计算粒子的权值改为采用的是巴斯距离。

 

　　工程环境：opencv2.3.1+vs2010

　　程序功能：驱动摄像头，待程序运行后用鼠标单击拖动选定需要跟踪的目标，然后程序会自动跟踪该目标。

　　其程序代码如下：

// particle_tracking.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <opencv2/core/core.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/highgui/highgui.hpp>
#include <stdio.h>
#include <iostream>

using namespace cv;
using namespace std;

Rect select;
bool select_flag=false;
bool tracking=false;//跟踪标志位
bool select_show=false;
Point origin;
Mat frame,hsv;
int after_select_frames=0;//选择矩形区域完后的帧计数

/****rgb空间用到的变量****/
//int hist_size[]={16,16,16};//rgb空间各维度的bin个数
//float rrange[]={0,255.0};
//float grange[]={0,255.0};
//float brange[]={0,255.0};
//const float *ranges[] ={rrange,grange,brange};//range相当于一个二维数组指针

/****hsv空间用到的变量****/
int hist_size[]={16,16,16};
float hrange[]={0,179.0};
float srange[]={0,255.0};
float vrange[]={0,255.0};

//int hist_size[]={32,32,32};
//float hrange[]={0,359.0.0};
//float srange[]={0,1.0};
//float vrange[]={0,1.0};
const float *ranges[]={hrange,srange,vrange};

int channels[]={0,1,2};

/****有关粒子窗口变化用到的相关变量****/
int A1=2;
int A2=-1;
int B0=1;
double sigmax=1.0;
double sigmay=0.5;
double sigmas=0.001;

/****定义使用粒子数目宏****/
#define PARTICLE_NUMBER 100 //如果这个数设定太大，经测试这个数字超过25就会报错，则在运行时将会出现错误

/****定义粒子结构体****/
typedef struct particle
{
    int orix,oriy;//原始粒子坐标
    int x,y;//当前粒子的坐标
    double scale;//当前粒子窗口的尺寸
    int prex,prey;//上一帧粒子的坐标
    double prescale;//上一帧粒子窗口的尺寸
    Rect rect;//当前粒子矩形窗口
    Mat hist;//当前粒子窗口直方图特征
    double weight;//当前粒子权值
}PARTICLE;

PARTICLE particles[PARTICLE_NUMBER];

/************************************************************************************************************************/
/****                            如果采用这个onMouse()函数的话，则可以画出鼠标拖动矩形框的4种情形                        ****/
/************************************************************************************************************************/
void onMouse(int event,int x,int y,int,void*)
{
    //Point origin;//不能在这个地方进行定义，因为这是基于消息响应的函数，执行完后origin就释放了，所以达不到效果。
    if(select_flag)
    {
        select.x=MIN(origin.x,x);//不一定要等鼠标弹起才计算矩形框，而应该在鼠标按下开始到弹起这段时间实时计算所选矩形框
        select.y=MIN(origin.y,y);
        select.width=abs(x-origin.x);//算矩形宽度和高度
        select.height=abs(y-origin.y);
        select&=Rect(0,0,frame.cols,frame.rows);//保证所选矩形框在视频显示区域之内

        //        rectangle(frame,select,Scalar(0,0,255),3,8,0);//显示手动选择的矩形框
    }
    if(event==CV_EVENT_LBUTTONDOWN)
    {
        select_flag=true;//鼠标按下的标志赋真值
        tracking=false;
        select_show=true;
        after_select_frames=0;//还没开始选择，或者重新开始选择，计数为0
        origin=Point(x,y);//保存下来单击是捕捉到的点
        select=Rect(x,y,0,0);//这里一定要初始化，因为在opencv中Rect矩形框类内的点是包含左上角那个点的，但是不含右下角那个点。
    }
    else if(event==CV_EVENT_LBUTTONUP)
    {
        select_flag=false;
        tracking=true;
        select_show=false;
        after_select_frames=1;//选择完后的那一帧当做第1帧
    }
}

/****粒子权值降序排列函数****/
int particle_decrease(const void *p1,const void *p2)
{
    PARTICLE* _p1=(PARTICLE*)p1;
    PARTICLE* _p2=(PARTICLE*)p2;
    if(_p1->weight<_p2->weight)
        return 1;
    else if(_p1->weight>_p2->weight)
        return -1;
    return 0;//相等的情况下返回0
}

int main(int argc, unsigned char* argv[])
{
    char c;
    Mat target_img,track_img;
    Mat target_hist,track_hist;
    PARTICLE *pParticle;

    /***打开摄像头****/
    VideoCapture cam(0);
    if (!cam.isOpened())
        return -1;

    /****建立窗口****/
    namedWindow("camera",1);//显示视频原图像的窗口

    /****捕捉鼠标****/
    setMouseCallback("camera",onMouse,0);

    while(1)
    {
        /****读取一帧图像****/
        cam>>frame;
        if(frame.empty())
            return -1;

        /****将rgb空间转换为hsv空间****/
        cvtColor(frame,hsv,CV_BGR2HSV);

        if(tracking)
        {

            if(1==after_select_frames)//选择完目标区域后
            {
                /****计算目标模板的直方图特征****/
                target_img=Mat(hsv,select);//在此之前先定义好target_img,然后这样赋值也行，要学会Mat的这个操作
                calcHist(&target_img,1,channels,Mat(),target_hist,3,hist_size,ranges);
                normalize(target_hist,target_hist);

                /****初始化目标粒子****/
                pParticle=particles;//指针初始化指向particles数组
                for(int x=0;x<PARTICLE_NUMBER;x++)
                {
                    pParticle->x=cvRound(select.x+0.5*select.width);//选定目标矩形框中心为初始粒子窗口中心
                    pParticle->y=cvRound(select.y+0.5*select.height);
                    pParticle->orix=pParticle->x;//粒子的原始坐标为选定矩形框(即目标)的中心
                    pParticle->oriy=pParticle->y;
                    pParticle->prex=pParticle->x;//更新上一次的粒子位置
                    pParticle->prey=pParticle->y;
                    pParticle->rect=select;
                    pParticle->prescale=1;
                    pParticle->scale=1;
                    pParticle->hist=target_hist;
                    pParticle->weight=0;
                    pParticle++;
                }
            }
            else if(2==after_select_frames)//从第二帧开始就可以开始跟踪了
            {
                double sum=0.0;
                pParticle=particles;
                RNG rng;//随机数产生器

                /****更新粒子结构体的大部分参数****/
                for(int i=0;i<PARTICLE_NUMBER;i++)
                {
                    int x,y;
                    int xpre,ypre;
                    double s,pres;

                    xpre=pParticle->x;
                    ypre=pParticle->y;
                    pres=pParticle->scale;

                    /****更新粒子的矩形区域即粒子中心****/
                    x=cvRound(A1*(pParticle->x-pParticle->orix)+A2*(pParticle->prex-pParticle->orix)+
                        B0*rng.gaussian(sigmax)+pParticle->orix);
                    pParticle->x=max(0,min(x,frame.cols-1));

                    y=cvRound(A1*(pParticle->y-pParticle->oriy)+A2*(pParticle->prey-pParticle->oriy)+
                        B0*rng.gaussian(sigmay)+pParticle->oriy);
                    pParticle->y=max(0,min(y,frame.rows-1));

                    s=A1*(pParticle->scale-1)+A2*(pParticle->prescale-1)+B0*(rng.gaussian(sigmas))+1.0;
                    pParticle->scale=max(1.0,min(s,3.0));

                    pParticle->prex=xpre;
                    pParticle->prey=ypre;
                    pParticle->prescale=pres;
            //        pParticle->orix=pParticle->orix;
            //        pParticle->oriy=pParticle->oriy;

                    //注意在c语言中，x-1.0，如果x是int型，则这句语法有错误,但如果前面加了cvRound(x-0.5)则是正确的
                    pParticle->rect.x=max(0,min(cvRound(pParticle->x-0.5*pParticle->scale*pParticle->rect.width),frame.cols));
                    pParticle->rect.y=max(0,min(cvRound(pParticle->y-0.5*pParticle->scale*pParticle->rect.height),frame.rows));
                    pParticle->rect.width=min(cvRound(pParticle->scale*pParticle->rect.width),frame.cols-pParticle->rect.x);
                    pParticle->rect.height=min(cvRound(pParticle->scale*pParticle->rect.height),frame.rows-pParticle->rect.y);

                    /****计算粒子区域的新的直方图特征****/
                    track_img=Mat(hsv,pParticle->rect);
                    calcHist(&track_img,1,channels,Mat(),track_hist,3,hist_size,ranges);
                    normalize(track_hist,track_hist);

                    /****更新粒子的权值****/
                    //            pParticle->weight=compareHist(target_hist,track_hist,CV_COMP_INTERSECT);
                    pParticle->weight=1.0-compareHist(target_hist,track_hist,CV_COMP_BHATTACHARYYA);//采用巴氏系数计算相似度

                    /****累加粒子权值****/
                    sum+=pParticle->weight;
                    pParticle++;
                }

                /****归一化粒子权重****/
                pParticle=particles;
                for(int i=0;i<PARTICLE_NUMBER;i++)
                {
                    pParticle->weight/=sum;
                    pParticle++;
                }

                /****根据粒子的权值降序排列****/
                pParticle=particles;
                qsort(pParticle,PARTICLE_NUMBER,sizeof(PARTICLE),&particle_decrease);

                /****根据粒子权重重采样粒子****/
                PARTICLE newParticle[PARTICLE_NUMBER];
                int np=0,k=0;
                for(int i=0;i<PARTICLE_NUMBER;i++)
                {
                    np=cvRound(pParticle->weight*PARTICLE_NUMBER);
                    for(int j=0;j<np;j++)
                    {
                        newParticle[k++]=particles[i];
                        if(k==PARTICLE_NUMBER)
                            goto EXITOUT;
                    }
                }
                while(k<PARTICLE_NUMBER)
                    newParticle[k++]=particles[0];
EXITOUT:
                for(int i=0;i<PARTICLE_NUMBER;i++)
                    particles[i]=newParticle[i];
            }//end else

            qsort(pParticle,PARTICLE_NUMBER,sizeof(PARTICLE),&particle_decrease);

            /****计算粒子期望，采用所有粒子位置的期望值做为跟踪结果****/
            /*Rect_<double> rectTrackingTemp(0.0,0.0,0.0,0.0);
            pParticle=particles;
            for(int i=0;i<PARTICLE_NUMBER;i++)
            {
                rectTrackingTemp.x+=pParticle->rect.x*pParticle->weight;
                rectTrackingTemp.y+=pParticle->rect.y*pParticle->weight;
                rectTrackingTemp.width+=pParticle->rect.width*pParticle->weight;
                rectTrackingTemp.height+=pParticle->rect.height*pParticle->weight;
                pParticle++;
            }*/

            /****计算最大权重目标的期望位置，作为跟踪结果****/
            Rect rectTrackingTemp(0,0,0,0);
            pParticle=particles;
            rectTrackingTemp.x=pParticle->x-0.5*pParticle->rect.width;
            rectTrackingTemp.y=pParticle->y-0.5*pParticle->rect.height;
            rectTrackingTemp.width=pParticle->rect.width;
            rectTrackingTemp.height=pParticle->rect.height;

            /****计算最大权重目标的期望位置，采用权值最大的1/4个粒子数作为跟踪结果****/
            /*Rect rectTrackingTemp(0,0,0,0);
            double weight_temp=0.0;
            pParticle=particles;
            for(int i=0;i<PARTICLE_NUMBER/4;i++)
            {
                weight_temp+=pParticle->weight;
                pParticle++;
            }
            pParticle=particles;
            for(int i=0;i<PARTICLE_NUMBER/4;i++)
            {
                pParticle->weight/=weight_temp;
                pParticle++;
            }
            pParticle=particles;
            for(int i=0;i<PARTICLE_NUMBER/4;i++)
            {
                rectTrackingTemp.x+=pParticle->rect.x*pParticle->weight;
                rectTrackingTemp.y+=pParticle->rect.y*pParticle->weight;
                rectTrackingTemp.width+=pParticle->rect.width*pParticle->weight;
                rectTrackingTemp.height+=pParticle->rect.height*pParticle->weight;
                pParticle++;
            }*/


            //创建目标矩形区域
            Rect tracking_rect(rectTrackingTemp);

            pParticle=particles;

            /****显示各粒子运动结果****/
            for(int m=0;m<PARTICLE_NUMBER;m++)
            {
                rectangle(frame,pParticle->rect,Scalar(255,0,0),1,8,0);
                pParticle++;
            }

            /****显示跟踪结果****/
            rectangle(frame,tracking_rect,Scalar(0,0,255),3,8,0);

            after_select_frames++;//总循环每循环一次，计数加1
            if(after_select_frames>2)//防止跟踪太长，after_select_frames计数溢出
                after_select_frames=2;
        }

        if(select_show)
            rectangle(frame,select,Scalar(0,0,255),3,8,0);//显示手动选择的矩形框
        //显示视频图片到窗口
        imshow("camera",frame);

        //    select.zeros();
        //键盘响应
        c=(char)waitKey(20);
        if(27==c)//ESC键
            return -1;
    }

    return 0;
}

 

　　但是该程序跟踪效果不是很好，不是特别稳定，可能跟粒子数目有关。因为程序还有bug，粒子数目设置太大后，编译通过。当运行时，手动选择目标区域后就自动退出了，即鼠标一拖动完后就在终端显示如下：

　　

 　　Debug程序后出现的错误提示：

　　

　　后面试了下，当粒子数目大于等于53时，运行程序，手选目标区域后程序没有自动退出，而是显示如下错误提示：

　　

　　因此，程序只有当粒子数目小于53才能正常运行，但是由于粒子数目太少所以效果不是特别的好。

　　等以后有时间来调试下内存方面的bug。