粒子滤波跟踪算法

#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, 180.0 };
float srange[] = { 0, 256.0 };
float vrange[] = { 0, 256.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;

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

    VideoWriter output_dst("demo.avi", CV_FOURCC('M', 'J', 'P', 'G'), 10, frame.size(), 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->rect.width), frame.cols - pParticle->rect.x);
                    pParticle->rect.height = min(cvRound(pParticle->rect.height), frame.rows - pParticle->rect.y);
                    //        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 rectTrackingTemp(0,0,0,0);
            pParticle=particles;
            for(int i=0;i<PARTICLE_NUMBER;i++)
            {
            rectTrackingTemp.x+=cvRound(pParticle->rect.x*pParticle->weight);
            rectTrackingTemp.y+=cvRound(pParticle->rect.y*pParticle->weight);
            pParticle++;
            }
            pParticle=particles;
            rectTrackingTemp.width = pParticle->rect.width;
            rectTrackingTemp.height = pParticle->rect.height;*/


            //创建目标矩形区域
            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);//显示手动选择的矩形框
        output_dst << frame;
        //显示视频图片到窗口
        imshow("camera", frame);

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

    return 0;
}

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值