meanshift算法学习（三）：自己实现meanshift

       有关meanShift的原理和数学推导，网上有大神提供了很详细的讲解文档，这里我推荐下面三篇个人认为比较好的文章。

https://wenku.baidu.com/view/5862334827d3240c8447ef40.html meanShift算法简介

http://blog.csdn.net/jinshengtao/article/details/30258833 基于meanShift的目标跟踪算法及实现

http://blog.csdn.net/anymake_ren/article/details/25484059 meanShift知识整理

       原理部分就不再重复造轮子了，这里直接把自己写的代码贴出来。代码实现的是读取一个视频序列，按下按键“p”后视频暂停，通过鼠标左键进行跟踪区域的选取，选取结束后按下按键“p”后视频继续播放同时开始跟踪。

#include "core/core.hpp"    
#include "highgui/highgui.hpp"    
#include "imgproc/imgproc.hpp"
#include "video/tracking.hpp"
#include<iostream>    
#include <numeric>
using namespace cv;    
using namespace std;    

Mat image;
bool leftButtonDown = false;// 鼠标左键是否按下
bool videoPauseFlag = false;// 是否暂停视频
bool trackingFlag = false;// 是否开始meanShift跟踪
Point pt1, pt2;// 记录选择区域的左上点/右下点
Rect rect;// 跟踪区域
vector<float> dstRegionDensity;// 目标跟踪区域的核函数估计密度向量
vector<float> testRegionDensity;// 候选区域的核函数估计密度向量
vector<float> w;// meanShift公式中的权值计算
int densityNum = 4096;// 对于每一帧图像的R/G/B三通道，每个通道按照值的大小分为16个区间，所以密度向量为16*16*16=4096维
void onMouse(int event,int x,int y,int flags ,void* ustc); //鼠标回调函数  
void calcKernelDensity(Mat imageSrc, vector<float>&density, int densityNum); // 计算图像的核函数估计密度向量
void meanShiftTracking(Mat& imageSrc, int iteration, double eps, Rect& rect);// meanShift算法跟踪


int main()
{
	// 打开视频文件
	VideoCapture cap("768X576.avi");
	if(!cap.isOpened())
	{
		cout<<"cannot open avi file"<<endl;
		return -1;
	}
	// 全局链表分配空间
	dstRegionDensity.resize(densityNum);
	testRegionDensity.resize(densityNum);
	w.resize(densityNum);

	double fps = cap.get(CV_CAP_PROP_FPS);// 获取图像帧率
	int pauseTime = (int)(1000.f/fps);
	namedWindow("video");
	setMouseCallback("video", onMouse);

	while(1)
	{
		if(waitKey(pauseTime) == 'p')
		{
			videoPauseFlag = !videoPauseFlag;
		}
		if(videoPauseFlag)// 暂停视频时不会更新图像帧
			continue;
		else
		{
			cap>>image;
			if(trackingFlag)
			{
				meanShiftTracking(image, 30, 0.2, rect);// meanShift跟踪
				int num = 0;
				
				/*for(int n = 0; n < densityNum; n++)
				{
					dstRegionDensity[n] = testRegionDensity[n];
				}*/
				rectangle(image, rect, Scalar(0, 0, 0));// 绘制出计算得到的跟踪位置

			}
		}
		imshow("video", image);

	}

}

void onMouse(int event,int x,int y,int flags ,void* ustc)
{
	Mat imageROI;
	// 鼠标左键按下获取区域起始点
	if(event == CV_EVENT_LBUTTONDOWN&&!trackingFlag)
	{
		leftButtonDown = true;
		pt1 = Point(x,y);
	}
	// 拖动选取区域并使用黑色线框显示选择区域
	else if(event == CV_EVENT_MOUSEMOVE && leftButtonDown)
	{
		Mat image_tmp;
		image.copyTo(image_tmp);
		pt2 = Point(x,y);
		rectangle(image_tmp, pt1, pt2, Scalar(0, 0, 0));
		imshow("video", image_tmp);
	}
	// 左键松开获取区域结束点，确定目标跟踪区域
	else if(event == CV_EVENT_LBUTTONUP && leftButtonDown)
	{
		leftButtonDown = false;
		pt2 = Point(x,y);
		image(Rect(pt1, pt2)).copyTo(imageROI);
		rect.x = std::min(pt1.x, pt2.x);
		rect.y = std::min(pt1.y, pt2.y);
		rect.width = pt1.x > pt2.x ? pt1.x - pt2.x : pt2.x - pt1.x;
		rect.height = pt1.y > pt2.y ? pt1.y - pt2.y : pt2.y - pt1.y;

		namedWindow("imageROI");
		imshow("imageROI", imageROI);
		// 计算目标跟踪区域的核函数估计密度向量
		calcKernelDensity(imageROI, dstRegionDensity, densityNum);
		waitKey(2000);
		destroyWindow("imageROI");
		trackingFlag = true;
	}

}


void calcKernelDensity(Mat imageSrc, vector<float>&density, int densityNum)
{
	/* 选取的核函数轮廓函数为k(x) = 1-x^2，其中x为图像中像素到图像中心位置的归一化距离*/
	int rows = imageSrc.rows;
	int cols = imageSrc.cols;
	float h = 0.25 * (rows*rows + cols*cols);// 带宽
	float k_sum = 0;
	for(int i = 0; i < densityNum; i++)
	{
		density[i] = 0;
	}
	for(int i = 0; i < rows; i++)
	{
		for(int j = 0; j < cols; j++)
		{
			int b,g,r,index;
			b = imageSrc.at<cv::Vec3b>(i,j)[0];
			g = imageSrc.at<cv::Vec3b>(i,j)[1];
			r = imageSrc.at<cv::Vec3b>(i,j)[2];
			index = b/16*256 + g/16*16 + r/16;// 获取像素点的索引值,0-4095
			float dis = ((i- rows/2)*(i- rows/2) + (j- cols/2)*(j- cols/2))/h;// x = sqrt(dis)
			float k = 1-dis;// k(x) = 1 - x^2; 
			density[index] += k;
			k_sum += k;
		}
	}

	for(int i = 0; i < densityNum; i++)
	{
		density[i] /= k_sum;// 密度归一化
	}
}


void meanShiftTracking(Mat& imageSrc, int iteration, double eps, Rect& rect)
{
	Mat imageROI;
	int num = 0;
	while(1)
	{
		imageSrc(rect).copyTo(imageROI);// 获取感兴趣区域

		Point2f pt_d;
		pt_d.x = pt_d.y = 0;
		float weightSum = 0;
		float h = 0.25 * (imageROI.rows*imageROI.rows + imageROI.cols*imageROI.cols);// 带宽

		calcKernelDensity(imageROI, testRegionDensity, densityNum);// 计算候选区域的核函数估计密度向量
		for(int i = 0; i < densityNum; i++)
		{
			if(testRegionDensity[i] != 0)
				w[i] = sqrt(dstRegionDensity[i] / testRegionDensity[i]);
			else
				w[i] = 0;// 计算迭代公式中的权值
		}
		for(int i = 0; i < imageROI.rows; i++)
		{
			for(int j = 0; j < imageROI.cols; j++)
			{
				int b,g,r, index;
				b = imageROI.at<cv::Vec3b>(i,j)[0];
				g = imageROI.at<cv::Vec3b>(i,j)[1];
				r = imageROI.at<cv::Vec3b>(i,j)[2];
				index = b/16*256 + g/16*16 + r/16;

				float dis = ((i- imageROI.rows/2)*(i- imageROI.rows/2)
					+ (j- imageROI.cols/2)*(j- imageROI.cols/2))/h;
				float weight_g = 2*sqrt(dis);
				pt_d.x += w[index]*weight_g*(j - imageROI.cols/2);
				pt_d.y += w[index]*weight_g*(i- imageROI.rows/2);
				weightSum += w[index]*weight_g;
			}
		}
		pt_d.x/=weightSum;
		pt_d.y/=weightSum;// 计算meanShift增量
		rect.x += pt_d.x;
		rect.y += pt_d.y;// 更新跟踪区域
		rect = rect&Rect(0, 0, imageSrc.cols, imageSrc.rows);// 保证跟踪区域位于图像内，这里的处理不一定合适

		float e = (pt_d.x*pt_d.x + pt_d.y * pt_d.y);
		if(e < eps)
			break;// 阈值判断
		num++;
		if(num > iteration)// 迭代次数判断
			break;
	}	
}

       跟踪效果如下图所示。

       代码和视频文件可以在点击打开链接下载。