opencv3/C++ meanshift与camshift目标跟踪

mean shift基本原理：

给定d维空间$\ R^d$中的n个样本点$\ x_{i},i=1,2,...,n,$在$\ x$点的mean shift向量的基本形式定义为：
$\ M_{h}(x)=\frac{1}{k}\sum_{x_{i}\in S_{k}}^{ }(x_{i}-x)$
其中，$\ S_h$是一个半径为h的高维球区域，满足以下关系的y点的集合，
$\ S_{h}(x)=\begin{Bmatrix} y:(y-x)^{T}(y-x)\leq h^{2}\end{Bmatrix}$
k表示在这n个样本点$\ x_i$中落入$\ S_h$区域中。
$\ (x-x_{i})$是样本点$\ x_i$相对于点$x$的偏移量，mean shift向量$\ M_{h}(x)$就是对落入区域$S_h$中的k个样本点相对于点x的偏移量求和然后再平均。
mean shift 示意图：

可以看出平均偏移量$\ M_{h}(x)$会指向样本分布最多的区域。公式中落入$\ S_h$的采样点，无论离中心x的远近，对最终计算的影响都一样。但现实跟踪过程中，较远的像素容易受背景或遮挡等因素的影响，因此靠近中心较近的像素应该更重要。故每个样本点的重要性应与其与中心的距离成反比。故引入核函数和权重系数来提高算法的鲁棒性。

核函数即窗口函数，在核估计中起到平滑的作用。常见的有：
Flat 核函数：
$\ k(x)=\left\{\begin{matrix} 1 ,if x \leq \lambda \\ 0 ,if x > \lambda \end{matrix}\right.$
Gaussian 核函数：
$\ k(x)=e^{-\frac{x}{2\sigma ^2}}$

部分核函数：

给出一个核函数：
$\ K(x-x_{i})$
该函数确定重新估计平均值的附近点的权重。当选择的搜索窗口类型为高斯核时：

$\ K(x-x_{i})=e^{-c\begin{Vmatrix} X-X_{i}\end{Vmatrix}^{2}}$

概率分布近似为：

$\ P(x)=\frac{1}{n} \sum_{i=1}^{n}K(x-x_{i})$

由K确定的窗口密度的加权平均值是：

$\ m(x)= \frac{\sum_{i=1}^{n}x_{i}g(\begin{Vmatrix} x-x_{i}\end{Vmatrix})}{\sum_{i=1}^{n} g(\begin{Vmatrix} x-x_{i}\end{Vmatrix})}$

其中：
$\ g(x)=-K^{'}(x)$

$P(x)$求梯度：

$\ \triangledown P(x)=\frac{c}{n}\sum_{i=1}^{n}\triangledown K_{i}=\frac{c}{n}\begin{bmatrix}\sum_{i=1}^{n}g_{i} \begin{bmatrix} \frac{\begin{Vmatrix} x-x_{i}\end{Vmatrix}}{b}\end{bmatrix}\end{bmatrix}\begin{bmatrix} \frac{\sum_{i=1}^{n}x_{i}g(\begin{Vmatrix} x-x_{i}\end{Vmatrix})}{\sum_{i=1}^{n} g(\begin{Vmatrix} x-x_{i}\end{Vmatrix})} - x\end{bmatrix}$

表达式构成如图：

mean shift算法主要应用于聚类、平滑和跟踪。

mean shift跟踪算法：

mean shift算法用于视觉跟踪时，将基于前一图像中的对象的颜色直方图在新图像中创建置信度图，并使用均值平移来找到靠近对象旧位置的置信度图的峰值。 置信度图是新图像上的概率密度函数，为新图像的每个像素指定一个概率，该概率是前一图像中的对象中出现的像素颜色的概率。

mean shift跟踪算法步骤：
① 选择搜索窗口，包括窗口的初始位置、大小、形状（对称或歪斜，矩形或圆心）、类型（均匀、多项式、指数或高斯）；
② 计算窗口的重心；
③ 将窗口的中心设置在计算出的重心处；
④ 返回②步，直到窗口位置不再变化。

设目标区域的中心为x，其中n个像素用$\ x_{i},i=1,...,n$表示，特征值个数为m，则目标模型的特征值$\ u=1,...,m$的概率密度估计为：
$\ \overrightarrow{q_{u}}= C \sum_{i=1}^{n}k \begin{bmatrix} \begin{Vmatrix} \frac{x-x_{i}}{h}\end{Vmatrix}^{2}\end{bmatrix} \sigma \begin{bmatrix} b(x_{i})-u\end{bmatrix}$

$\ \overrightarrow{q}=\begin{Bmatrix} q_{u} \end{Bmatrix}_{u=1,...,m}$
$\ \sum_{u=1}^{m}q_{u}=1$
其中，k为权值系数，赋予靠近中心的像素一个较大权值，远离中心的像素给定较小的权值。
$\ \sigma \begin{bmatrix} b(x_{i})-u\end{bmatrix}$的作用是判断目标区域中像素值是否属于第u个特征值。属于时为1，不属于时为0；$\ b(x_i)$为灰度值索引函数。
C为一标准化的常量系数:

$\ C= \frac{1}{ \sum_{i=1}^{n}k \begin{bmatrix} \begin{Vmatrix} \frac{x-x_{i}}{h}\end{Vmatrix}^{2}\end{bmatrix}}$

这样可以使$\ \sum_{u=1}^{m}q_{u}=1$。

于是可以得到基于图像灰度特征的颜色直方图。

运动目标在第二帧以后的每帧中可能包含目标的区域称为候选区域，其中心坐标即核函数的中心坐标y。该区域中的像素用$\ x_{i},i=1,...,n_k$表示；候选模型的特征值$\ u=1,...,m$的概率密度为：
$\ \overrightarrow{p_{u}}(y)= C_{h} \sum_{i=1}^{n_{k}}k \begin{bmatrix} \begin{Vmatrix} \frac{y-x_{i}}{h}\end{Vmatrix}^{2}\end{bmatrix} \sigma \begin{bmatrix} b(x_{i})-u\end{bmatrix}$

$\ \overrightarrow{p}(y)=\begin{Bmatrix} p_{u} (y)\end{Bmatrix}_{u=1,...,m}$
$\ \sum_{u=1}^{m}p_{u}=1$

$\ C_h$为标准化常量：

$\ C_{h}= \frac{1}{ \sum_{i=1}^{n_k}k \begin{bmatrix} \begin{Vmatrix} \frac{y-x_{i}}{h}\end{Vmatrix}^{2}\end{bmatrix}}$

Bhattacharyya系数

利用相似函数描述目标模型和候选模型之间的相似度，理想情况下两个模型的概率分布一样。使用Bhattacharyya系数作为相似函数。
令$\ BC(p,q)$为Bhattacharyya系数。
Bhattacharyya系数是衡量两个统计样本之间重叠量或相对接近度的指标。
对于离散概率分布：$\ BC(p,q)=\sum_{x\in X}^{ }\sqrt{p(x)q(x)}$
对于连续概率分布：$\ BC(p,q)=\int\sqrt{p(x)q(x)}dx$
另，Bhattacharyya距离为:$\ D_{B}(p,q)=-ln(BC(p,q))$

模板区域：$\ \overrightarrow{q}=(q_{1},...,q_{m})$
候选区域：$\ \overrightarrow{p}(y)=(p_{1}(y),...,p_{m}(y))$
相似性函数：$\ f(y)=f[ \overrightarrow{p}(y), \overrightarrow{q}]=\sum_{u=1}^{m}\sqrt{p_{u}(y)q_{u}}$

Bhattacharyya系数的值在0到1之间，值越大表示两个模型月相似。跟踪时选择使Bhattacharyya系数最大的候选区域作为当前帧中的目标位置。

camshift算法

camshift算法就是将meanshift算法扩展到连续图像序列。camshift将视频的所有帧做meanshift运算，并将上一帧的结果（搜索窗的大小和中心），作为下一帧meanshift算法搜索窗的初始值。然后一直迭代下去，实现对目标的跟踪。

函数CamShift()参数说明：

//查找对象中心，大小和方向。
RotatedRect CamShift(
InputArray probImage,//对象直方图的反向投影
CV_IN_OUT Rect& window,//初始搜索窗口
TermCriteria criteria//底层meanShift的停止标准
);

OpenCV3 camshift跟踪

#include<opencv2/opencv.hpp>
#include<opencv2/tracking.hpp>
using namespace cv;

int main()
{
    VideoCapture capture;
    Mat frame;
    //保存目标轨迹  
    std::vector<Point> pt; 
    capture.open(0);
    if (!capture.isOpened())
    {
        printf("can not open camera \n");
        return -1;
    }
    namedWindow("input", WINDOW_AUTOSIZE);
    namedWindow("output", WINDOW_AUTOSIZE);
    capture.read(frame);
    if (frame.empty())
        return -1;
    Rect2d first = selectROI("output", frame);
    Rect selectionROI;
    selectionROI.width = first.width;
    selectionROI.height = first.height;
    selectionROI.x = first.x;
    selectionROI.y = first.y;
    printf("x= %d, y=%d, width=%d, height=%d",selectionROI.x,selectionROI.y,selectionROI.width,selectionROI.height);

    Mat mask, hist, backproject;
    int bins = 120;
    Mat drawImg = Mat::zeros(300, 300, CV_8UC3);

    while (capture.read(frame))
    {
        Mat hsvimage;
        cvtColor(frame, hsvimage, CV_BGR2HSV);
        inRange(hsvimage, Scalar(25, 43, 46), Scalar(35, 256, 256), mask);
        Mat hue = Mat(hsvimage.size(), hsvimage.depth());
        int channels[] = {0, 0};
        mixChannels(&hsvimage, 1, &hue, 1, channels, 1);

        //ROI直方图计算
        Mat roi(hue, first);
        Mat maskroi(mask, first);
        float hrange[] = {0, 180};
        const float* hranges = hrange;
        //直方图
        calcHist(&roi, 1, 0, maskroi, hist, 1, &bins, &hranges);
        normalize(hist, hist, 0, 255, NORM_MINMAX);

        int binw = drawImg.cols/ bins;
        Mat colorIndex = Mat(1, bins, CV_8UC3);
        for (int i = 0; i < bins; i++)
        {
            colorIndex.at<Vec3b>(0, i) = Vec3b(saturate_cast<uchar>(i * 180 / bins), 255, 255);
        }
        cvtColor(colorIndex, colorIndex, COLOR_HSV2BGR);
        for (int i = 0; i < bins; i++)
        {
            int val = saturate_cast<int>(hist.at<float>(i)*drawImg.rows/255);
            rectangle(drawImg, Point(i*binw, drawImg.rows), Point((i+1)*binw, drawImg.rows * val), Scalar(colorIndex.at<Vec3b>(0, i)), -1, 8, 0);
        }

        //计算直方图的反投影
        calcBackProject(&hue, 1, 0, hist, backproject, &hranges);
        backproject &= mask;
        RotatedRect trackBox = CamShift(backproject, selectionROI, TermCriteria((TermCriteria::COUNT | TermCriteria::EPS), 10, 1));
        Rect rect; 
        rect.x = trackBox.center.x - trackBox.size.width/2.0;
        rect.y = trackBox.center.y - trackBox.size.height/2.0;
        rect.width = trackBox.size.width;
        rect.height = trackBox.size.height;

        rectangle(frame, rect, Scalar(255, 255, 0),3);

        pt.push_back(Point(rect.x+rect.width/2,rect.y+rect.height/2));  
        for(int i=0;i<pt.size()-1;i++) 
        {    
            line(frame,pt[i],pt[i+1],Scalar(0,255,0),2.5);    
        } 
        imshow("input", frame);
        imshow("output", drawImg);
        waitKey(10);
    }

    capture.release();
    return 0;
}