目标跟踪学习笔记1

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

#include "stdafx.h"
/************************************************************************/
/* 
Description:    基本的粒子滤波目标跟踪
Author:            Yang Xian
Email:            yang_xian521@163.com
Version:        2011-11-2
History:
*/
/************************************************************************/
#include <iostream>    // for standard I/O
#include <string>   // for strings
#include <iomanip>  // for controlling float print precision
#include <sstream>  // string to number conversion

#include <opencv2/imgproc/imgproc.hpp>  
#include <opencv2/core/core.hpp>        // Basic OpenCV structures (cv::Mat, Scalar)
#include <opencv2/highgui/highgui.hpp>  // OpenCV window I/O

using namespace cv;
using namespace std;

// 以下这些参数对结果影响很大，而且也会根据视频内容，会对结果有很大的影响
const int PARTICLE_NUM = 25;    // 粒子个数
// 粒子放入的相关区域
const double A1 = 2.0;
const double A2 = -1.0;
const double B0 = 1.0;
// 高斯随机数sigma参数
const double SIGMA_X = 1.0;
const double SIGMA_Y = 0.5;
const double SIGMA_SCALE = 0.001;

// 粒子结构体
typedef struct particle {
    double x;            // 当前x坐标
    double y;            // 当前y坐标
    double scale;        // 窗口比例系数
    double xPre;            // x坐标预测位置
    double yPre;            // y坐标预测位置
    double scalePre;        // 窗口预测比例系数
    double xOri;            // 原始x坐标
    double yOri;            // 原始y坐标
    Rect rect;            // 原始区域大小
    MatND hist;            // 粒子区域的特征直方图
    double weight;        // 该粒子的权重
} PARTICLE;

Mat hsv;    // hsv色彩空间的输入图像
Mat roiImage;    // 目标区域
MatND roiHist;    // 目标区域直方图
Mat img;    // 输出的目标图像
PARTICLE particles[PARTICLE_NUM];    // 粒子

int nFrameNum = 0;

bool bSelectObject = false;    // 区域选择标志
bool bTracking = false;        // 开始跟踪标志
Point origin;    // 鼠标按下时的点位置
Rect selection;// 感兴趣的区域大小

// 直方图相关参数，特征的选取也会对结果影响巨大
// Quantize the hue to 30 levels
// and the saturation to 32 levels
// value to 10 levels
int hbins = 180, sbins = 256, vbin = 10;
int histSize[] = {hbins, sbins, vbin};
// hue varies from 0 to 179, see cvtColor
float hranges[] = { 0, 180 };
// saturation varies from 0 (black-gray-white) to 255 (pure spectrum color)
float sranges[] = { 0, 256 };
// value varies from 0 (black-gray-white) to 255 (pure spectrum color)
float vranges[] = { 0, 256 };
const float* ranges[] = {hranges, sranges, vranges};
// we compute the histogram from the 0-th and 1-st channels
int channels[] = {0, 1, 2};

// 鼠标响应函数，得到选择的区域，保存在selection
void onMouse(int event, int x, int y, int, void*)
{
    if( bSelectObject )
    {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);

        selection &= Rect(0, 0, img.cols, img.rows);
    }

    switch (event)
    {
    case CV_EVENT_LBUTTONDOWN:
        origin = Point(x,y);
        selection = Rect(x,y,0,0);
        bSelectObject = true;
        bTracking = false;
        break;
    case CV_EVENT_LBUTTONUP:
        bSelectObject = false;
    //    if( selection.width > 0 && selection.height > 0 )
        bTracking = true;
        nFrameNum = 0;
        break;
    }
}

// 快速排序算法排序函数
int particle_cmp(const void* p1,const void* p2)
{
    PARTICLE* _p1 = (PARTICLE*)p1;
    PARTICLE* _p2 = (PARTICLE*)p2;

    if(_p1->weight < _p2->weight)
        return 1;    //按照权重降序排序
    if(_p1->weight > _p2->weight)
        return -1;
    return 0;
}

const char* keys = 
{
    "{1|  | 0 | camera number}"
};

int main(int argc, const char **argv)//这里char **argv前必须用const，why？
{
    int delay = 30;    // 控制播放速度
    char c;    // 键值

    /*读取avi文件*/
    VideoCapture captRefrnc("IndoorGTTest1.avi");    // 视频文件

    /*打开摄像头*/
    //VideoCapture captRefrnc;
//CommandLineParser parser(argc, argv, keys);//命令解析器函数
//int camNum = parser.get<int>("1");   
//captRefrnc.open(camNum);

    if ( !captRefrnc.isOpened())
    {
        return -1;
    }

    // Windows
    const char* WIN_RESULT = "Result";
    namedWindow(WIN_RESULT, CV_WINDOW_AUTOSIZE);
    //namedWindow(WIN_RESULT, 0);
//      namedWindow("Result",0);
// 鼠标响应函数
//setMouseCallback(WIN_RESULT, onMouse, 0);
    setMouseCallback("Result", onMouse, 0);

    Mat frame;    //视频的每一帧图像

    bool paused = false;
    PARTICLE * pParticles = particles;//particles为可装PARTICLE_NUM个PARTICLE结构体的数组，所以pParticles为指向其数组的指针

    while(true) //Show the image captured in the window and repeat
    {
        if(!paused)
        {
            captRefrnc >> frame;
            if(frame.empty())
                break;
        }

        frame.copyTo(img);    // 接下来的操作都是对src的


// 选择目标后进行跟踪
        if (bTracking == true)//鼠标操作选完后
        {
            if(!paused)
            {                
                nFrameNum++;//帧数计数器
                cvtColor(img, hsv, CV_BGR2HSV);
                Mat roiImage(hsv, selection);    // 目标区域，这个构造函数第二个参数表示截取selection部分

                if (nFrameNum == 1)    //选择目标后的第一帧需要初始化
                {
                    // step 1: 提取目标区域特征，难道其目标特征就是其色调的直方图分布？
                    calcHist(&roiImage, 1, channels, Mat(), roiHist, 3, histSize, ranges);
                    normalize(roiHist, roiHist);    // 归一化L2

// step 2: 初始化particle
                    pParticles = particles;
                    for (int i=0; i<PARTICLE_NUM; i++)
                    {
                        pParticles->x = selection.x + 0.5 * selection.width;
                        pParticles->y = selection.y + 0.5 * selection.height;
                        pParticles->xPre = pParticles->x;
                        pParticles->yPre = pParticles->y;
                        pParticles->xOri = pParticles->x;
                        pParticles->yOri = pParticles->y;
                        pParticles->rect = selection;
                        pParticles->scale = 1.0;
                        pParticles->scalePre = 1.0;
                        pParticles->hist = roiHist;
                        pParticles->weight = 0;
                        pParticles++;
                    }
                }
                else //不是第一帧
                {                    
                    pParticles = particles;    
                    RNG rng;//随机数序列产生器
                    for (int i=0; i<PARTICLE_NUM; i++)
                    {
                        // step 3: 求particle的transition，粒子结构中的参数全部更新过
                        double x, y, s;

                        pParticles->xPre = pParticles->x;
                        pParticles->yPre = pParticles->y;
                        pParticles->scalePre = pParticles->scale;

                        x = A1 * (pParticles->x - pParticles->xOri) + A2 * (pParticles->xPre - pParticles->xOri) +
                            B0 * rng.gaussian(SIGMA_X) + pParticles->xOri;//以当前点为中心产生高斯分布的粒子
                        pParticles->x = std::max(0.0, std::min(x, img.cols-1.0));//其实就是x，只是考虑了边界在内而已


                        y = A1 * (pParticles->y - pParticles->yOri) + A2 * (pParticles->yPre - pParticles->yOri) +
                            B0 * rng.gaussian(SIGMA_Y) + pParticles->yOri;
                        pParticles->y = std::max(0.0, std::min(y, img.rows-1.0));

                        s = A1 * (pParticles->scale - 1.0) + A2 * (pParticles->scalePre - 1.0) +
                            B0 * rng.gaussian(SIGMA_SCALE) + 1.0;
                        pParticles->scale = std::max(0.1, std::min(s, 3.0));
                        // rect参数有待考证
                        pParticles->rect.x = std::max(0, std::min(cvRound(pParticles->x - 0.5 * pParticles->rect.width * pParticles->scale), img.cols-1));        // 0 <= x <= img.rows-1
                        pParticles->rect.y = std::max(0, std::min(cvRound(pParticles->y - 0.5 * pParticles->rect.height * pParticles->scale), img.rows-1));    // 0 <= y <= img.cols-1
                        pParticles->rect.width = std::min(cvRound(pParticles->rect.width * pParticles->scale), img.cols - pParticles->rect.x);
                        pParticles->rect.height = std::min(cvRound(pParticles->rect.height * pParticles->scale), img.rows - pParticles->rect.y);
                        // Ori参数不改变

// step 4: 求particle区域的特征直方图
                        Mat imgParticle(img, pParticles->rect);
                        calcHist(&imgParticle, 1, channels, Mat(), pParticles->hist, 3, histSize, ranges);
                        normalize(pParticles->hist, pParticles->hist);    // 归一化L2

// step 5: 特征的比对,更新particle权重
//compareHist()函数返回2个直方图之间的相似度，因为参数为CV_COMP_INTERSECT，所以返回的是最小直方图值之和
                        pParticles->weight = compareHist(roiHist, pParticles->hist, CV_COMP_INTERSECT);//其差值直接作为其权值

                        pParticles++;    
                    }

                    // step 6: 归一化粒子权重
                    double sum = 0.0;
                    int i;

                    pParticles = particles;
                    for(i=0; i<PARTICLE_NUM; i++)
                    {
                        sum += pParticles->weight;
                        pParticles++;
                    }
                    pParticles = particles;
                    for(i=0; i<PARTICLE_NUM; i++)
                    {
                        pParticles->weight /= sum;
                        pParticles++;
                    }

                    // step 7: resample根据粒子的权重的后验概率分布重新采样
                    pParticles = particles;
                    //                     PARTICLE* newParticles = new PARTICLE[sizeof(PARTICLE) * PARTICLE_NUM];
                    PARTICLE newParticles[PARTICLE_NUM];
                    int np, k = 0;
                    
                    //qsort()函数为对数组进行快速排序,其中第4个参数表示的是排序是升序还是降序
                    qsort(pParticles, PARTICLE_NUM, sizeof(PARTICLE), &particle_cmp);//这里采用的是降序排列
                    for(int i=0; i<PARTICLE_NUM; i++)
                    {
                        np = cvRound(particles[i].weight * PARTICLE_NUM);//权值高的优先重采样
                        for(int j=0; j<np; j++)
                        {
                            newParticles[k++] = particles[i];
                            if(k == PARTICLE_NUM)//重采样后达到了个数要求则直接跳出
                                goto EXITOUT;
                        }
                    }
                    while(k < PARTICLE_NUM)
                    {
                        newParticles[k++] = particles[0];//个数不够时，将权值最高的粒子重复给
                    }

EXITOUT:
                    for (int i=0; i<PARTICLE_NUM; i++)
                    {
                        particles[i] = newParticles[i];
                    }

                }// end else

                qsort(pParticles, PARTICLE_NUM, sizeof(PARTICLE), &particle_cmp);

                // step 8: 计算粒子的期望，作为跟踪结果
                Rect_<double> rectTrackingTemp(0.0, 0.0, 0.0, 0.0);
                pParticles = particles;
                for (int i=0; i<PARTICLE_NUM; i++)
                {
                    rectTrackingTemp.x += pParticles->rect.x * pParticles->weight;//坐标加上权重的偏移值
                    rectTrackingTemp.y += pParticles->rect.y * pParticles->weight;
                    rectTrackingTemp.width += pParticles->rect.width * pParticles->weight;//宽度也要加上权值的偏移值
                    rectTrackingTemp.height += pParticles->rect.height * pParticles->weight;
                    pParticles++;
                }
                Rect rectTracking(rectTrackingTemp);    // 跟踪结果

// 显示各粒子的运动
                for (int i=0; i<PARTICLE_NUM; i++)
                {
                    rectangle(img, particles[i].rect, Scalar(255,0,0));
                }
                // 显示跟踪结果
                rectangle(img, rectTracking, Scalar(0,0,255), 3);

            }
        }// end Tracking

//         imshow(WIN_SRC, frame);
        imshow(WIN_RESULT, img);

        c = (char)waitKey(delay);
        if( c == 27 )
            break;
        switch(c)
        {
        case ‘p‘://暂停键
            paused = !paused;
            break;
        default:
            ;
        }
    }// end while
}

// 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,180.0};
float srange[]={0,256.0};
float vrange[]={0,256.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 50 //如果这个数设定太大，比如100，则在运行时将会出现错误

/****定义粒子结构体****/
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;
                    double s;

                    /****更新粒子的矩形区域即粒子中心****/
                    pParticle->prex=pParticle->x;
                    pParticle->prey=pParticle->y;
                    pParticle->prescale=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));

                    //注意在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=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 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;
}