机器人视觉项目：视觉检测识别+机器人跟随（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<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函数：当按下鼠标左键时，这个点就是起始点，按住鼠标左键移动鼠标，会选择感兴趣区域，松开鼠标左键，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<<"支持向量个数："<<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 * pAlphaData = svm.get_alpha_vector();//返回SVM的决策函数中的alpha向量
        for(int i=0; i<supportVectorNum; i++)
        {
            alphaMat.at<float>(0,i) = pAlphaData[i];
        }

        //计算-(alphaMat * supportVectorMat),结果放到resultMat中
        //gemm(alphaMat, supportVectorMat, -1, 0, 1, resultMat);//不知道为什么加负号？
        resultMat = -1 * alphaMat * supportVectorMat;

        //得到最终的setSVMDetector(const vector<float>& detector)参数中可用的检测子

        //将resultMat中的数据复制到数组myDetector中
        for(int i=0; i<DescriptorDim; i++)
        {
            myDetector.push_back(resultMat.at<float>(0,i));
        }
        //最后添加偏移量rho，得到检测子
        myDetector.push_back(svm.get_rho());
        cout<<"检测子维数："<<myDetector.size()<<endl;
        hog.setSVMDetector(myDetector);
        ofstream fout("HOGDetectorForOpenCV.txt");
        for(int i=0; i<myDetector.size(); i++)
        {
            fout<<myDetector[i]<<endl;
        }
        printf("Start the tracking process\n");

    }
    //行人检测
    void peopleDetect()
    {
        if(has_dectect_people)
            return;
        vector<Rect> 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一起作为是否重新检测行人追踪的标记。