标题：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)1.2 online.launch(with real robot) 2. utility.h3. traversability_filter.cpp3.1 话题的订阅与发布3.2 各函数作用3.2.1 cloudHandler3.2.2 extractRawCloud3.2.3 transformCloud3.2.4 cloud2Matrix3.2.5 applyFilter3.2.6 extractFilteredCloud3.2.7 downsampleCloud3.2.8 predictCloudBGK3.2.9 publishCloud3.2.10 publishLaserScan3.2.11 main function 4. traversability_map.cpp4.1 话题的订阅与发布4.2 各函数作用4.2.1 Register Cloud4.2.2 Traversability Calculation4.2.3 Occupancy Map (local)4.2.4 Point Cloud4.2.5 Main Function

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;

(2) negativeCurbFilter 同理，对第0~第scanNumCurbFilter(8)每一线，遍历该线上的每一点，如果该点和其前后rangeCompareNeighborNum(3)个邻居点相比，满足两点的高度差大于0.1，并且两点间的距离小于等于1.0，则将该点标记为障碍物点。

// height diff greater than threshold, might be a negative curb if (laserCloudMatrix[i][j].z - laserCloudMatrix[i][k].z > 0.1 && pointDistance(laserCloudMatrix[i][j], laserCloudMatrix[i][k]) <= 1.0){ obstacleMatrix.at<int>(i, j) = 1; break; }

(3) slopeFilter 对第0~第scanNumSlopeFilter(10)每一线，遍历计算同一水平位置上下两线对应点的俯仰角，作为倾斜角(slope angle)，如果倾斜角不在[-filterAngleLimit(-20)，filterAngleLimit(20)]之间，则将该点标记为障碍物点。

float diffX = laserCloudMatrix[i+1][j].x - laserCloudMatrix[i][j].x; float diffY = laserCloudMatrix[i+1][j].y - laserCloudMatrix[i][j].y; float diffZ = laserCloudMatrix[i+1][j].z - laserCloudMatrix[i][j].z; float angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY)) * 180 / M_PI; // Slope angle is larger than threashold, mark as obstacle point if (angle < -filterAngleLimit || angle > filterAngleLimit){ obstacleMatrix.at<int>(i, j) = 1; continue; }

3.2.6 extractFilteredCloud

根据obstacleMatrix，更新点云的占用信息(occupancy,0~100)，并保存到laserCloudOut中，同时提取障碍物点云，并保存到laserCloudObstacles中，最后发布者pubCloudVisualHiRes发布点云laserCloudOut，话题名称为"/filtered_pointcloud_visual_high_res"。

3.2.7 downsampleCloud

网格化降采样点云，每个网格cell只存储一个点，网格中心的x,y坐标，以及划分到该网格中所有点云的最大高度maxHeight，另外，点的intensity属性存储的是占用信息：0～100，并将点保存到laserCloudOut，最后发布者pubCloudVisualLowRes发布点云laserCloudOut，话题名称为"/filtered_pointcloud_visual_low_res"。

3.2.8 predictCloudBGK

贝叶斯广义核推理推断网格点的高程信息和占用信息，主要包括以下步骤： (1) 以localMapOrigin为起点，遍历x,y方向索引0~filterHeightMapArrayLength范围内的每个网格点，如果该点未被观测到，则记为testPoint. (2) 在以testPoint为中心的长度为2*kernelGridLength的方形网格范围内，选取在边界范围内，且被观测到的网格点数据，作为训练数据。 (3) 由训练数据进行BGK推断testPoint的高度信息(elevation)和占用信息(occupancy，0~100). (4) 更新testPoint的高度信息(elevation)和占用信息(occupancy，0~100)，并将其保存到laserCloudOut。 循环直到遍历完。

3.2.9 publishCloud

发布者pubCloud发布点云laserCloudOut，laserCloudOut中包含了经过BGK推断的点云信息，话题名称为""/filtered_pointcloud""。

3.2.10 publishLaserScan

通过函数updateLaserScan更新LaserScan，并且通过发布者pubLaserScan发布LaserScan，话题名称为"/pointcloud_2_laserscan"。

3.2.11 main function

int main(int argc, char** argv){ ros::init(argc, argv, "traversability_mapping"); TraversabilityFilter TFilter; ROS_INFO("\033[1;32m---->\033[0m Traversability Filter Started."); ros::spin(); return 0; }

4. traversability_map.cpp

4.1 话题的订阅与发布

(1) 订阅话题：

ros::Subscriber subFilteredGroundCloud; // subscribe to traversability filter subFilteredGroundCloud = nh.subscribe<sensor_msgs::PointCloud2>("/filtered_pointcloud", 5, &TraversabilityMapping::cloudHandler, this);

(2) 发布话题：

ros::Publisher pubOccupancyMapLocal; ros::Publisher pubOccupancyMapLocalHeight; ros::Publisher pubElevationCloud; // publish local occupancy and elevation grid map pubOccupancyMapLocal = nh.advertise<nav_msgs::OccupancyGrid> ("/occupancy_map_local", 5); pubOccupancyMapLocalHeight = nh.advertise<elevation_msgs::OccupancyElevation> ("/occupancy_map_local_height", 5); // publish elevation map for visualization pubElevationCloud = nh.advertise<sensor_msgs::PointCloud2> ("/elevation_pointcloud", 5);

4.2 各函数作用

4.2.1 Register Cloud

(1) cloudHandler

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){ // Lock thread std::lock_guard<std::mutex> lock(mtx); // Get Robot Position if (getRobotPosition() == false) return; // Convert Point Cloud pcl::fromROSMsg(*laserCloudMsg, *laserCloud); // Register New Scan updateElevationMap(); // publish local occupancy grid map publishMap(); }

(2) updateElevationMap()

根据从话题"/filtered_pointcloud"中接收到的点云，更新高程图。

void updateElevationMap(){ int cloudSize = laserCloud->points.size(); for (int i = 0; i < cloudSize; ++i){ laserCloud->points[i].z -= 0.2; // for visualization updateElevationMap(&laserCloud->points[i]); } }

(3) updateElevationMap(PointType *point)

给定数据点，更新高程图，主要包括以下几个函数： updateCellElevation，使用卡尔曼滤波的方法更新网格单元的高程信息。 updateCellOccupancy，使用log_odds更新网格单元的占用信息。 updateCellObservationTime，更新网格单元的被观测次数。

void updateElevationMap(PointType *point){ // Find point index in global map grid_t thisGrid; if (findPointGridInMap(&thisGrid, point) == false) return; // Get current cell pointer mapCell_t *thisCell = grid2Cell(&thisGrid); // update elevation updateCellElevation(thisCell, point); // update occupancy updateCellOccupancy(thisCell, point); // update observation time updateCellObservationTime(thisCell); }

(4) updateCellElevation

使用卡尔曼滤波的方法更新网格单元的高程信息。

void updateCellElevation(mapCell_t *thisCell, PointType *point){ // Kalman Filter: update cell elevation using Kalman filter // https://www.cs.cornell.edu/courses/cs4758/2012sp/materials/MI63slides.pdf // cell is observed for the first time, no need to use Kalman filter if (thisCell->elevation == -FLT_MAX){ thisCell->elevation = point->z; thisCell->elevationVar = pointDistance(robotPoint, *point); return; } // Predict: float x_pred = thisCell->elevation; // x = F * x + B * u float P_pred = thisCell->elevationVar + 0.01; // P = F*P*F + Q // Update: float R_factor = (thisCell->observeTimes > 20) ? 10 : 1; float R = pointDistance(robotPoint, *point) * R_factor; // measurement noise: R, scale it with dist and observed times float K = P_pred / (P_pred + R);// Gain: K = P * H^T * (HPH + R)^-1 float y = point->z; // measurement: y float x_final = x_pred + K * (y - x_pred); // x_final = x_pred + K * (y - H * x_pred) float P_final = (1 - K) * P_pred; // P_final = (I - K * H) * P_pred // Update cell thisCell->updateElevation(x_final, P_final); }

(5) updateCellOccupancy

使用log_odds更新网格单元的占用信息。

void updateCellOccupancy(mapCell_t *thisCell, PointType *point){ // Update log_odds float p; // Probability of being occupied knowing current measurement. if (point->intensity == 100) p = p_occupied_when_laser; else p = p_occupied_when_no_laser; thisCell->log_odds += std::log(p / (1 - p)); if (thisCell->log_odds < -large_log_odds) thisCell->log_odds = -large_log_odds; else if (thisCell->log_odds > large_log_odds) thisCell->log_odds = large_log_odds; // Update occupancy float occupancy; if (thisCell->log_odds < -max_log_odds_for_belief) occupancy = 0; else if (thisCell->log_odds > max_log_odds_for_belief) occupancy = 100; else occupancy = (int)(lround((1 - 1 / (1 + std::exp(thisCell->log_odds))) * 100)); // update cell thisCell->updateOccupancy(occupancy); }

4.2.2 Traversability Calculation

(1) TraversabilityThread

void TraversabilityThread(){ ros::Rate rate(10); // Hz while (ros::ok()){ traversabilityMapCalculation(); rate.sleep(); } }

(2) traversabilityMapCalculation

计算新观测到的网格单元的可通行性，计算步骤如下：

对observingList中的每个cell，通过函数findNeighborElevations计算其邻居cell的位置信息，并保存;找到邻居cell中的最大高度和最小高度，计算高度差，如果高度差大于阈值filterHeightLimit，将该点的intensity置为100，更新网格单元的占用信息。计算倾斜角slopeAngle，如果倾斜角大于阈值filterAngleLimit，将该点的intensity置为100，更新网格单元的占用信息。 void traversabilityMapCalculation(){ // no new scan, return if (observingList1.size() == 0) return; observingList2 = observingList1; observingList1.clear(); int listSize = observingList2.size(); for (int i = 0; i < listSize; ++i){ mapCell_t *thisCell = observingList2[i]; // convert this cell to a point for convenience PointType thisPoint; thisPoint.x = thisCell->xyz->x; thisPoint.y = thisCell->xyz->y; thisPoint.z = thisCell->xyz->z; // too far, not accurate if (pointDistance(thisPoint, robotPoint) >= traversabilityCalculatingDistance) continue; // Find neighbor cells of this center cell vector<float> xyzVector = findNeighborElevations(thisCell); if (xyzVector.size() <= 2) continue; Eigen::MatrixXf matPoints = Eigen::Map<const Eigen::Matrix<float, -1, -1, Eigen::RowMajor>>(xyzVector.data(), xyzVector.size() / 3, 3); // min and max elevation float minElevation = matPoints.col(2).minCoeff(); float maxElevation = matPoints.col(2).maxCoeff(); float maxDifference = maxElevation - minElevation; if (maxDifference > filterHeightLimit){ thisPoint.intensity = 100; updateCellOccupancy(thisCell, &thisPoint); continue; } // find slope Eigen::MatrixXf centered = matPoints.rowwise() - matPoints.colwise().mean(); Eigen::MatrixXf cov = (centered.adjoint() * centered); cv::eigen2cv(cov, matCov); // copy data from eigen to cv::Mat cv::eigen(matCov, matEig, matVec); // find eigenvalues and eigenvectors for the covariance matrix // float slopeAngle = std::acos(std::abs(matVec.at<float>(2, 2))) / M_PI * 180; // // float occupancy = 1.0f / (1.0f + exp(-(slopeAngle - filterAngleLimit))); // float occupancy = 0.5 * (slopeAngle / filterAngleLimit) // + 0.5 * (maxDifference / filterHeightLimit); // if (slopeAngle > filterAngleLimit || maxDifference > filterHeightLimit) // thisPoint.intensity = 100; updateCellOccupancy(thisCell, &thisPoint); } }

4.2.3 Occupancy Map (local)

(1) publishMap

发布占用栅格地图，高程图

void publishMap(){ // Publish Occupancy Grid Map and Elevation Map pubCount++; if (pubCount > visualizationFrequency){ pubCount = 1; publishLocalMap(); publishTraversabilityMap(); } }

(2) publishLocalMap

发布占用栅格地图信息 发布者pubOccupancyMapLocalHeight发布话题occupancyMap2DHeight，话题名称为"/occupancy_map_local"; 发布者pubOccupancyMapLocal发布话题occupancyMap2DHeight.occupancy，话题名称为"/occupancy_map_local_height".

4.2.4 Point Cloud

(1) publishTraversabilityMap

发布者 pubElevationCloud发布点云laserCloudElevation，话题名称为"/elevation_pointcloud"

4.2.5 Main Function

int main(int argc, char** argv){ ros::init(argc, argv, "traversability_mapping"); TraversabilityMapping tMapping; std::thread predictionThread(&TraversabilityMapping::TraversabilityThread, &tMapping); ROS_INFO("\033[1;32m---->\033[0m Traversability Mapping Started."); ROS_INFO("\033[1;32m---->\033[0m Traversability Mapping Scenario: %s.", urbanMapping == true ? "\033[1;31mUrban\033[0m" : "\033[1;31mTerrain\033[0m"); ros::spin(); return 0; }