【代码阅读】基于贝叶斯广义核推理的可通行区域建图

本文链接：https://blog.youkuaiyun.com/Zlp19970106/article/details/108947709

标题：Bayesian Generalized Kernel Inference forTerrain Traversability Mapping
作者：Tixiao Shan，Jinkun Wang，Brendan Englot and Kevin Doherty
来源：CoRL 2018
代码:
https://github.com/TixiaoShan/traversability_mapping/tree/master/traversability_mapping

1. launch文件

1.1 offline.launch(in simulation)

主要分为6部分：
(1) 参数“/use_sim_time”置为true
(2) 发布TF变换
(3) 运行lego_loam lidar odometry节点
(4) 运行rviz节点
(5) 运行traversability mapping节点
(6) 运行move base

1.2 online.launch(with real robot)

主要分为5部分：
(1) 参数“/use_sim_time”置为false
(2) 发布TF变换
(3) 运行lego_loam lidar odometry节点
(4) 运行traversability mapping节点
(5) 运行move base

2. utility.h

主要定义了5个结构体和一些参数。
(1) struct grid_t
定义了每个sub_map在map中的索引，每个cell在sub_map中的索引。

(2) struct mapCell_t
定义了最小的网格单元，包括点的位置信息PointType *xyz，占用信息occupancy，高程信息elevation以及占用信息的更新，高程信息的更新等。

(3) struct childMap_t
sub_map的定义，submap是二维的正方形网格，由很多cell构成。

(4) struct state_t
定义了机器人的状态信息

(5) struct neighbor_t
定义了PRM邻居节点信息

3. traversability_filter.cpp

3.1 话题的订阅与发布

(1) 订阅话题：

 ros::Subscriber subCloud;
 subCloud = nh.subscribe<sensor_msgs::PointCloud2>("/full_cloud_info", 5, &TraversabilityFilter::cloudHandler, this);

订阅的话题为"/full_cloud_info"，来自lego_loam，接收到的点云为包含有效位置信息及距离信息的原始激光点云。

(2) 发布话题：

 ros::Publisher pubCloud;
 ros::Publisher pubCloudVisualHiRes;
 ros::Publisher pubCloudVisualLowRes;
 ros::Publisher pubLaserScan;

 pubCloud = nh.advertise<sensor_msgs::PointCloud2> ("/filtered_pointcloud", 5);
 pubCloudVisualHiRes = nh.advertise<sensor_msgs::PointCloud2> ("/filtered_pointcloud_visual_high_res", 5);
 pubCloudVisualLowRes = nh.advertise<sensor_msgs::PointCloud2> ("/filtered_pointcloud_visual_low_res", 5);
 pubLaserScan = nh.advertise<sensor_msgs::LaserScan> ("/pointcloud_2_laserscan", 5);

在发布的四个话题中：
"/filtered_pointcloud_visual_high_res"，发布的是经过positiveCurbFilter、negativeCurbFilter、slopeFilter筛选后的点云信息；

"/filtered_pointcloud_visual_low_res"，发布的是经过网格化降采样的点云信息，每个网格cell只存储一个点，网格中心的x,y坐标，以及划分到该网格中所有点云的最大高度maxHeight，另外，点的intensity属性存储的是障碍物信息：0～100。

"/filtered_pointcloud"，在经过网格化降采样的点云信息中，加入了经过BGK高程(elevation)预测和占用信息(occupancy)预测的点云信息，并发布。

"/pointcloud_2_laserscan"，将障碍物点云转化为laser scan，并发布。

3.2 各函数作用

3.2.1 cloudHandler

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
   
        
        extractRawCloud(laserCloudMsg);

        if (transformCloud() == false) return;

        cloud2Matrix();

        applyFilter();

        extractFilteredCloud();

        downsampleCloud();

        predictCloudBGK();

        publishCloud();

        publishLaserScan();

        resetParameters();
    }

3.2.2 extractRawCloud

提取原始点云，并保存range信息到rangeMatrix中，并将obstacleMatrix中的所有元素初始化为0，表示free。

3.2.3 transformCloud

从监听得到的TF变换中，得到机器人的位robotPoint，并将点云转换到map坐标系下。

3.2.4 cloud2Matrix

将点云按竖直方向线数和水平方向，存储到二维矩阵laserCloudMatrix中。

3.2.5 applyFilter

    void applyFilter(){
   

        if (urbanMapping == true){
   
            positiveCurbFilter();
            negativeCurbFilter();
        }

        slopeFilter();
    }

点云滤波，主要包括以下三个函数：

(1) positiveCurbFilter
对第0~第scanNumCurbFilter(8)每一线：
依次计算相邻两个点的距离差(range difference)，并保存；
遍历该线上的每一点，如果同时满足以下4个条件，则将其标记为障碍物点，

该点在传感器有效测距范围之内;

该点的前后rangeCompareNeighborNum(3)个邻居点均存在有效的距离信息;

该点的前后rangeCompareNeighborNum(3)个邻居点，相邻两点的距离差为单调递增或递减;

该点的第一个邻居点和最后一个邻居点的距离差的绝对值，大于某个阈值。

if (abs(rangeMatrix.at<float>(i, j-rangeCompareNeighborNum) - rangeMatrix.at<float>(i, j+rangeCompareNeighborNum)) /rangeMatrix.at<float>(i, j) < 0.03)
                    continue;
 // if "continue" is not used at this point, it is very likely to be an obstacle point
obstacleMatrix.at<int>(i, j) = 1;