机器人视觉项目：视觉检测识别+机器人跟随（17）
参考一个实例行人检测
在ubuntu+ros环境下，利用RGBD采集数据给小车，实现行人跟随。
原作者开源的例子是出现一个窗口，用鼠标选择一个区域做kcf跟随，选择的物体不受限制，物体移动后，机器人随即跟随。
博主实现的是用opencv行人检测的demo，替换原作者的例子。该demo在opencv-2.4.13/samples/cpp/peopledetect.cpp下。
先认识一下原作者的例子中的runtracker.cpp，其中的ImageConverter类是核心，首先对接收/发送主题的Publisher/Subscrible初始化，
设置回调函数。
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
depth_sub_是接受深度图的subscribe，执行depthCb回掉函数，depthCb作用就是计算距离和方向。
将手动选择感兴趣区域改为自动选择感兴趣区域，是在imageCb函数中修改。
imageCb中 cv::setMouseCallback(RGB_WINDOW, onMouse, 0);监听鼠标操作，如果鼠标不动，程序不会往下执行。onMouse为鼠标监听回调。
要实现自动选择肯定就不能用这个了，将其注掉。
onMouse函数：当按下鼠标左键时，这个点就是起始点，按住鼠标左键移动鼠标，会选择感兴趣区域，松开鼠标左键，bRenewROI = true;修改标志，
新的roi区域selectRect已经产生。在imageCb中程序继续执行，初始化KCFTracker，开始追踪。
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<<"支持向量个数："<(i,j) = pSVData[j];
}
}
//将alpha向量的数据复制到alphaMat中
double * pAlphaData = svm.get_alpha_vector();//返回SVM的决策函数中的alpha向量
for(int i=0; i(0,i) = pAlphaData[i];
}
//计算-(alphaMat * supportVectorMat),结果放到resultMat中
//gemm(alphaMat, supportVectorMat, -1, 0, 1, resultMat);//不知道为什么加负号？
resultMat = -1 * alphaMat * supportVectorMat;
//得到最终的setSVMDetector(const vector& detector)参数中可用的检测子
//将resultMat中的数据复制到数组myDetector中
for(int i=0; i(0,i));
}
//最后添加偏移量rho，得到检测子
myDetector.push_back(svm.get_rho());
cout<<"检测子维数："< found, found_filtered;
double t = (double)getTickCount();
hog.detectMultiScale(rgbimage, found, 0, Size(8,8), Size(32,32), 1.05, 2);
t = (double)getTickCount() - t;
//printf("tdetection time = %gms\n", t*1000./cv::getTickFrequency());
size_t i, j;
printf("found.size==%d",found.size());
for( i = 0; i < found.size(); i++ )
{
Rect r = found[i];
for( j = 0; j < found.size(); j++ )
if( j != i && (r & found[j]) == r)
break;
if( j == found.size() )
found_filtered.push_back(r);
}
Rect r ;
for( i = 0; i < found_filtered.size(); i++ )
{
r = found_filtered[i];
// the HOG detector returns slightly larger rectangles than the real objects.
// so we slightly shrink the rectangles to get a nicer output.
r.x += cvRound(r.width*0.1);
r.width = cvRound(r.width*0.8);
r.y += cvRound(r.height*0.07);
r.height = cvRound(r.height*0.8);
//rectangle(rgbimage, r.tl(), r.br(), cv::Scalar(0,255,0), 3);
//printf("r.x==%d,y==%d,width==%d,height==%d\n",r.x,r.y,r.width,r.height);
}
//防止误检测
if(r.width>100&&r.height>350){
has_dectect_people=true;
selectRect.x = r.x+(r.width-roi_width)/2;
selectRect.y = r.y+(r.height-roi_height)/2;
selectRect.width = roi_width;
selectRect.height = roi_height;
printf("selectRect.x==%d,y==%d,width==%d,height==%d\n",selectRect.x,selectRect.y,selectRect.width,selectRect.height);
}//imshow(RGB_WINDOW, rgbimage);
}
检测到人后，人所在的区域是一个矩形，在矩形区域内取其中间100*100的矩形为感兴趣区域。检测到人后将has_dectect_people置为true，
使其不会再次检测。设置bRenewROI = true;select_flag = true; select_flag：当追踪目标未消失时，为true，消失时为false，
与bRenewROI一起作为是否重新检测行人追踪的标记。