视频跟踪——meanshift算法

部分内容转载于：

http://blog.csdn.net/google19890102/article/details/51030884

http://www.cnblogs.com/liqizhou/archive/2012/05/12/2497220.html

meanShift,均值漂移，在聚类、图像平滑、分割、跟踪等方面有着广泛的应用。meanShift这个概念最早是由Fukunage在1975年提出的，其最初的含义正如其名：偏移的均值向量；但随着理论的发展，meanShift的含义已经发生了很多变化。如今，我们说的meanShift算法，一般是指一个迭代的步骤，即先算出当前点的偏移均值，然后以此为新的起始点，继续移动，直到满足一定的结束条件。

meanShift的两大特色：

1.定义了核函数。
2.增加了权重系数。

先解释一下核函数：在Mean Shift算法中引入核函数的目的是使得随着样本与被偏移点的距离不同，其偏移量对均值偏移向量的贡献也不同。核函数是机器学习中常用的一种方式。核函数的定义如下所示：

X表示一个d维的欧式空间，x是该空间中的一个点x={x1,x2,x3⋯,xd}，其中，x的模∥x∥2=xxT，R表示实数域，如果一个函数K:X→R存在一个剖面函数k:[0,∞]→R，即 
K(x)=k(∥x∥2)
并且满足： 
(1)、k是非负的 
(2)、k是非增的 
(3)、k是分段连续的 
那么，函数K(x)就称为核函数。

而权重系数使得不同样本点的重要性不一样，这大大扩展了meanShift的应用范围。

在Mean Shift算法中，实际上是利用了概率密度，求得概率密度的局部最优解。
概率密度梯度

对一个概率密度函数f(x)，已知d维空间中n个采样点xi,i=1,⋯,n，f(x)的核函数估计(也称为Parzen窗估计)为： 

f^(x)=∑ni=1K(xi−xh)w(xi)hd∑ni=1w(xi)

其中 
w(xi)⩾0是一个赋给采样点xi的权重 
K(x)是一个核函数
概率密度函数f(x)的梯度▽f(x)的估计为
▽f^(x)=2∑ni=1(x−xi)k′(∥∥xi−xh∥∥2)w(xi)hd+2∑ni=1w(xi)
令g(x)=−k′(x)，G(x)=g(∥x∥2)，则有：
▽f^(x)=2∑ni=1(xi−x)G(∥∥xi−xh∥∥2)w(xi)hd+2∑ni=1w(xi)=2h2⎡⎣⎢∑ni=1G(xi−xh)w(xi)hd∑ni=1w(xi)⎤⎦⎥⎡⎣⎢∑ni=1(xi−x)G(∥∥xi−xh∥∥2)w(xi)∑ni=1G(xi−xh)w(xi)⎤⎦⎥
其中，第二个方括号中的就是Mean Shift向量，其与概率密度梯度成正比。
Mean Shift向量的修正
Mh(x)=∑ni=1G(∥∥xi−xh∥∥2)w(xi)xi∑ni=1G(xi−xh)w(xi)−x
记：mh(x)=∑ni=1G(∥∥xi−xh∥∥2)w(xi)xi∑ni=1G(xi−xh)w(xi)，则上式变成：
Mh(x)=mh(x)+x
这与梯度上升的过程一致。
Mean Shift算法流程
Mean Shift算法的算法流程如下：
计算mh(x)
令x=mh(x)
如果∥mh(x)−x∥<ε，结束循环，否则，重复上述步骤

基于均值漂移的目标跟踪算法通过分别计算目标区域和候选区域内像素的特征值概率得到关于目标模型和候选模型的描述，然后利用相似函数度量初始帧目标模型和当前帧的候选模版的相似性，选择使相似函数最大的候选模型并得到关于目标模型的Meanshift向量，这个向量正是目标由初始位置向正确位置移动的向量。由于均值漂移算法的快速收敛性，通过不断迭代计算Meanshift向量，算法最终将收敛到目标的真实位置，达到跟踪的目的。
     下面通过图示直观的说明MeanShift跟踪算法的基本原理。如下图所示：目标跟踪开始于数据点xi0（空心圆点xi0，xi1，…，xiN表示的是中心点，上标表示的是的迭代次数，周围的黑色圆点表示不断移动中的窗口样本点，虚线圆圈代表的是密度估计窗口的大小）。箭头表示样本点相对于核函数中心点的漂移向量，平均的漂移向量会指向样本点最密集的方向，也就是梯度方向。因为 Meanshift 算法是收敛的，因此在当前帧中通过反复迭代搜索特征空间中样本点最密集的区域，搜索点沿着样本点密度增加的方向“漂移”到局部密度极大点点xiN，也就是被认为的目标位置，从而达到跟踪的目的，MeanShift 跟踪过程结束。

相关代码：

#include "stdafx.h"
#include "cv.h"
#include "highgui.h"
#define  u_char unsigned char
#define  DIST 0.5
#define  NUM 20

//全局变量
bool pause = false;
bool is_tracking = false;
CvRect drawing_box;
IplImage *current;
double *hist1, *hist2;
double *m_wei;																	//权值矩阵
double C = 0.0;																   //归一化系数

void init_target(double *hist1, double *m_wei, IplImage *current)
{
	IplImage *pic_hist = 0;
	int t_h, t_w, t_x, t_y;
	double h, dist;
	int i, j;
	int q_r, q_g, q_b, q_temp;
	
	t_h = drawing_box.height;
	t_w = drawing_box.width;
	t_x = drawing_box.x;
	t_y = drawing_box.y;

	h = pow(((double)t_w)/2,2) + pow(((double)t_h)/2,2);			//带宽
	pic_hist = cvCreateImage(cvSize(300,200),IPL_DEPTH_8U,3);     //生成直方图图像

	//初始化权值矩阵和目标直方图
	for (i = 0;i < t_w*t_h;i++)
	{
		m_wei[i] = 0.0;
	}

	for (i=0;i<4096;i++)
	{
		hist1[i] = 0.0;
	}

	for (i = 0;i < t_h; i++)
	{
		for (j = 0;j < t_w; j++)
		{
			dist = pow(i - (double)t_h/2,2) + pow(j - (double)t_w/2,2);
			m_wei[i * t_w + j] = 1 - dist / h; 
			//printf("%f\n",m_wei[i * t_w + j]);
			C += m_wei[i * t_w + j] ;
		}
	}

	//计算目标权值直方
	for (i = t_y;i < t_y + t_h; i++)
	{
		for (j = t_x;j < t_x + t_w; j++)
		{
			//rgb颜色空间量化为16*16*16 bins
			q_r = ((u_char)current->imageData[i * current->widthStep + j * 3 + 2]) / 16;
			q_g = ((u_char)current->imageData[i * current->widthStep + j * 3 + 1]) / 16;
			q_b = ((u_char)current->imageData[i * current->widthStep + j * 3 + 0]) / 16;
			q_temp = q_r * 256 + q_g * 16 + q_b;
			hist1[q_temp] =  hist1[q_temp] +  m_wei[(i - t_y) * t_w + (j - t_x)] ;
		}
	}

	//归一化直方图
	for (i=0;i<4096;i++)
	{
		hist1[i] = hist1[i] / C;
		//printf("%f\n",hist1[i]);
	}

	//生成目标直方图
	double temp_max=0.0;

	for (i = 0;i < 4096;i++)			//求直方图最大值，为了归一化
	{
		//printf("%f\n",val_hist[i]);
		if (temp_max < hist1[i])
		{
			temp_max = hist1[i];
		}
	}
	//画直方图
	CvPoint p1,p2;
	double bin_width=(double)pic_hist->width/4096;
	double bin_unith=(double)pic_hist->height/temp_max;

	for (i = 0;i < 4096; i++)
	{
		p1.x = i * bin_width;
		p1.y = pic_hist->height;
		p2.x = (i + 1)*bin_width;
		p2.y = pic_hist->height - hist1[i] * bin_unith;
		//printf("%d,%d,%d,%d\n",p1.x,p1.y,p2.x,p2.y);
		cvRectangle(pic_hist,p1,p2,cvScalar(0,255,0),-1,8,0);
	}
	cvSaveImage("hist1.jpg",pic_hist);
	cvReleaseImage(&pic_hist);
}

void MeanShift_Tracking(IplImage *current)
{
	int num = 0, i = 0, j = 0;
	int t_w = 0, t_h = 0, t_x = 0, t_y = 0;
	double *w = 0, *hist2 = 0;
	double sum_w = 0, x1 = 0, x2 = 0,y1 = 2.0, y2 = 2.0;
	int q_r, q_g, q_b;
	int *q_temp;
	IplImage *pic_hist = 0;

	t_w = drawing_box.width;
	t_h = drawing_box.height;
	
	pic_hist = cvCreateImage(cvSize(300,200),IPL_DEPTH_8U,3);     //生成直方图图像
	hist2 = (double *)malloc(sizeof(double)*4096);
	w = (double *)malloc(sizeof(double)*4096);
	q_temp = (int *)malloc(sizeof(int)*t_w*t_h);

	while ((pow(y2,2) + pow(y1,2) > 0.5)&& (num < NUM))
	{
		num++;
		t_x = drawing_box.x;
		t_y = drawing_box.y;
		memset(q_temp,0,sizeof(int)*t_w*t_h);
		for (i = 0;i<4096;i++)
		{
			w[i] = 0.0;
			hist2[i] = 0.0;
		}

		for (i = t_y;i < t_h + t_y;i++)
		{
			for (j = t_x;j < t_w + t_x;j++)
			{
				//rgb颜色空间量化为16*16*16 bins
				q_r = ((u_char)current->imageData[i * current->widthStep + j * 3 + 2]) / 16;
				q_g = ((u_char)current->imageData[i * current->widthStep + j * 3 + 1]) / 16;
				q_b = ((u_char)current->imageData[i * current->widthStep + j * 3 + 0]) / 16;
				q_temp[(i - t_y) *t_w + j - t_x] = q_r * 256 + q_g * 16 + q_b;
				hist2[q_temp[(i - t_y) *t_w + j - t_x]] =  hist2[q_temp[(i - t_y) *t_w + j - t_x]] +  m_wei[(i - t_y) * t_w + j - t_x] ;
			}
		}

		//归一化直方图
		for (i=0;i<4096;i++)
		{
			hist2[i] = hist2[i] / C;
			//printf("%f\n",hist2[i]);
		}
		//生成目标直方图
		double temp_max=0.0;

		for (i=0;i<4096;i++)			//求直方图最大值，为了归一化
		{
			if (temp_max < hist2[i])
			{
				temp_max = hist2[i];
			}
		}
		//画直方图
		CvPoint p1,p2;
		double bin_width=(double)pic_hist->width/(4368);
		double bin_unith=(double)pic_hist->height/temp_max;

		for (i = 0;i < 4096; i++)
		{
			p1.x = i * bin_width;
			p1.y = pic_hist->height;
			p2.x = (i + 1)*bin_width;
			p2.y = pic_hist->height - hist2[i] * bin_unith;
			cvRectangle(pic_hist,p1,p2,cvScalar(0,255,0),-1,8,0);
		}
		cvSaveImage("hist2.jpg",pic_hist);
	
		for (i = 0;i < 4096;i++)
		{
			if (hist2[i] != 0)
			{
				w[i] = sqrt(hist1[i]/hist2[i]);
			}else
			{
				w[i] = 0;
			}
		}
			
		sum_w = 0.0;
		x1 = 0.0;
		x2 = 0.0;

		for (i = 0;i < t_h; i++)
		{
			for (j = 0;j < t_w; j++)
			{
				//printf("%d\n",q_temp[i * t_w + j]);
				sum_w = sum_w + w[q_temp[i * t_w + j]];
				x1 = x1 + w[q_temp[i * t_w + j]] * (i - t_h/2);
				x2 = x2 + w[q_temp[i * t_w + j]] * (j - t_w/2);
			}
		}
		y1 = x1 / sum_w;
		y2 = x2 / sum_w;
		
		//中心点位置更新
		drawing_box.x += y2;
		drawing_box.y += y1;

		//printf("%d,%d\n",drawing_box.x,drawing_box.y);
	}
	free(hist2);
	free(w);
	free(q_temp);
	//显示跟踪结果
	cvRectangle(current,cvPoint(drawing_box.x,drawing_box.y),cvPoint(drawing_box.x+drawing_box.width,drawing_box.y+drawing_box.height),CV_RGB(255,0,0),2);
	cvShowImage("Meanshift",current);
	//cvSaveImage("result.jpg",current);
	cvReleaseImage(&pic_hist);
}

void onMouse( int event, int x, int y, int flags, void *param )
{
	if (pause)
	{
		switch(event)
		{
		case CV_EVENT_LBUTTONDOWN: 
			//the left up point of the rect
			drawing_box.x=x;
			drawing_box.y=y;
			break;
		case CV_EVENT_LBUTTONUP:
			//finish drawing the rect (use color green for finish)
			drawing_box.width=x-drawing_box.x;
			drawing_box.height=y-drawing_box.y;
			cvRectangle(current,cvPoint(drawing_box.x,drawing_box.y),cvPoint(drawing_box.x+drawing_box.width,drawing_box.y+drawing_box.height),CV_RGB(255,0,0),2);
			cvShowImage("Meanshift",current);
			
			//目标初始化
			hist1 = (double *)malloc(sizeof(double)*16*16*16);
			m_wei =  (double *)malloc(sizeof(double)*drawing_box.height*drawing_box.width);
			init_target(hist1, m_wei, current);
			is_tracking = true;
			break;
		}
		return;
	}
}



int _tmain(int argc, _TCHAR* argv[])
{
	CvCapture *capture=cvCreateFileCapture("test.avi");
	current = cvQueryFrame(capture);
	char res[20];
	int nframe = 0;

	while (1)
	{	
	/*	sprintf(res,"result%d.jpg",nframe);
		cvSaveImage(res,current);
		nframe++;*/
		if(is_tracking)
		{
			MeanShift_Tracking(current);
		}

		int c=cvWaitKey(1);
		//暂停
		if(c == 'p') 
		{
			pause = true;
			cvSetMouseCallback( "Meanshift", onMouse, 0 );
		}
		while(pause){
			if(cvWaitKey(0) == 'p')
				pause = false;
		}
		cvShowImage("Meanshift",current);
		current = cvQueryFrame(capture); //抓取一帧
	}

	cvNamedWindow("Meanshift",1);
	cvReleaseCapture(&capture);
	cvDestroyWindow("Meanshift");
	return 0;
}