使用opencv实现直线提取

void mapCallback(const nav_msgs::OccupancyGrid& msg)
{
    ROS_INFO("I heard frame_id: [%s]", msg.header.frame_id.c_str());
    resolution = msg.info.resolution;
    width = msg.info.width;
    height = msg.info.height;
    originx = msg.info.origin.position.x;
    originy = msg.info.origin.position.y;
    mymap = msg;
    
    map_ = cv::Mat(height,width,CV_8U);
    for(int i = 0;i< height;i++)
    {
        for(int j=0;j< width;j++)
        {
            if(mymap.data[i*width+j] == 100)
            {
                map_.at(i,j) = 255;
            }
            else
            {
                map_.at(i,j) = 0;
            }
        }
    }
    thin(map_,map_,2);
    std::vector l;
    cv::HoughLinesP(map_,l,1,0.1,10, 0,10);
    ROS_INFO("detect line:%d",l.size());
    for(int i=0;i< l.size();i++)
    {
        cv::Vec4i I = l[i];
        cv::line(map_,cv::Point(I[0]+100,I[1]+100),cv::Point(I[2]+100,I[3]+100),cv::Scalar(128),1,CV_AA);
    }/**/
    return;
}