ros：kcf算法+行人检测 = 让机器人自动识别并追踪行人

实现目标：机器人检测到有人走过来，迎上去并开始追踪。
追踪算法使用kcf算法，关于kcf追踪的ros库在github地址 https://github.com/TianyeAlex/tracker_kcf_ros，kcf算法是目前追踪算法中比较好的，程序跑起来后效果也是不错的。我能力有限，在这里不作介绍。有兴趣的可以去研究一下。这里主要讲一下在次基础上添加行人检测，做到自动追踪。
训练库地址：http://download.youkuaiyun.com/detail/yiranhaiziqi/9711174，下载后放到src目录下。

追踪的代码结构
作者将kcf算法封装起来，在runtracker.cpp里面调用。

程序跑起来的效果

出现一个窗口，用鼠标左键选中一个区域作为感兴趣区域，之后机器人会跟踪这个区域。例如，选中画面中的椅子，移动椅子之后，机器人会跟随移动。选中画面中的人或者人的某个部位都可以实现人物跟踪。我要想实现自动追踪，就是把鼠标选择跟踪物变成自动选择跟踪物，这里的跟踪物就是行人。
首先要先实现行人检测，在opencv中，有行人检测的demo，路径在opencv-2.4.13/samples/cpp/peopledetect.cpp。接下来做的就是把代码结合起来。
简单介绍一下runtracker.cpp。
ImageConverter类是核心
初始化我们要接受/发送主题的Publisher 和Subscriber，设置相应的回掉函数。

image_sub_ = it_.subscribe("/camera/rgb/image_rect_color", 1,&ImageConverter::imageCb, this);
		depth_sub_ = it_.subscribe("/camera/depth/image", 1,&ImageConverter::depthCb, this);
		pub = nh_.advertise<geometry_msgs::Twist>("/mobile_base/mobile_base_controller/cmd_vel", 1000);

image_sub_是接受rgb图的subscribe，执行imageCb回掉函数，imageCb主要是将摄像头的数据显示在窗口中，选择感兴趣区域。
depth_sub_是接受深度图的subscribe，执行depthCb回掉函数，depthCb作用就是计算距离和方向。
了解到这里之后，要将手动选择感兴趣区域改为自动选择感兴趣区域，必然是在imageCb函数中修改。
imageCb中 cv::setMouseCallback(RGB_WINDOW, onMouse, 0);监听鼠标操作，如果鼠标不动，程序不会往下执行。onMouse为鼠标监听回调。要实现自动选择肯定就不能用这个了，将其注掉。
再来看下onMouse函数做了什么事

void onMouse(int event, int x, int y, int, void*)
{
	if (select_flag)
	{
		selectRect.x = MIN(origin.x, x);
		selectRect.y = MIN(origin.y, y);
		selectRect.width = abs(x - origin.x);
		selectRect.height = abs(y - origin.y);
		selectRect &= cv::Rect(0, 0, rgbimage.cols, rgbimage.rows);
	}
	if (event == CV_EVENT_LBUTTONDOWN)
	{
		bBeginKCF = false;
		select_flag = true;
		origin = cv::Point(x, y);
		selectRect = cv::Rect(x, y, 0, 0);
	}
	else if (event == CV_EVENT_LBUTTONUP)
	{
		select_flag = false;
		bRenewROI = true;
	}
}

当按下鼠标左键时，这个点就是起始点，按住鼠标左键移动鼠标，会选择感兴趣区域，松开鼠标左键，bRenewROI = true;修改标志，表示新的roi区域selectRect已经产生。在imageCb中程序继续执行，初始化KCFTracker，开始追踪。
到这里基本的流程已经比较清晰了，接下来开始将行人检测代替手动选择roi区域。

preparePeopleDetect()函数是初始化检测，
peopleDetect()函数是开始检测。

void preparePeopleDetect()
	{
		has_dectect_people = false;
		//hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());//使用默认的训练数据，下面是使用自己的训练数据。
		MySVM svm;
		string path = ros::package::getPath("track_pkg")+"/src/12000neg_2400pos.xml";
		printf("path === %s",path.c_str());
		//svm.load("/home/server/catkin_ws/src/tracker_kcf_ros/src/track_pkg/src/12000neg_2400pos.xml");
		svm.load(path.c_str());
		DescriptorDim = svm.get_var_count();//特征向量的维数，即HOG描述子的维数
		int supportVectorNum = svm.get_support_vector_count();//支持向量的个数
		cout<<"支持向量个数："<<supportVectorNum<<endl;

		Mat alphaMat = Mat::zeros(1, supportVectorNum, CV_32FC1);//alpha向量，长度等于支持向量个数
		Mat supportVectorMat = Mat::zeros(supportVectorNum, DescriptorDim, CV_32FC1);//支持向量矩阵
		Mat resultMat = Mat::zeros(1, DescriptorDim, CV_32FC1);//alpha向量乘以支持向量矩阵的结果

		//将支持向量的数据复制到supportVectorMat矩阵中
		for(int i=0; i<supportVectorNum; i++)
		{
			const float * pSVData = svm.get_support_vector(i);//返回第i个支持向量的数据指针
			for(int j=0; j<DescriptorDim; j++)
			{
				supportVectorMat.at<float>(i,j) = pSVData[j];
			}
		}

		//将alpha向量的数据复制到alphaMat中
		double * pAlp