autoware.universe源码略读(3.18)--perception:probabilistic_occupancy_grid_map

Overview

这个包的作用是根据拥有障碍物的概率来输出occupancy grid map的

在galatic分支这里，用到了两种类型，分别是Pointcloud based occupancy grid map和Laserscan based occupancy grid map，这部分可以结合高博的书《自动驾驶与机器人中的SLAM 技术》来看，可能理解的会更好一些

laserscan_based_occupancy_grid_map

算法原理

其基本思路是利用二维激光扫描和射线跟踪来创建经过时间序列处理的占用网格图。

1. 该节点进行激光线扫描，并用一帧绘制占位网格图。其中用到了Bresenham算法
   

2. 这里可选择接收障碍物点云和原始点云，并将其反映在占位网格图中。因为激光线扫描只使用极坐标系中的最前景点，所以会丢弃很多信息。因此，占用网格图几乎是一个未知单元。
   因此，障碍物点云和原始点云用于反映占用网格图中被判定为地面的区域和被判定为障碍物的区域。
   

黑点和红点代表原始点云，红点代表障碍物点云。换句话说，黑色点表示地面，红色点云表示障碍物。灰色单元表示未知单元。

3. 利用之前的占用网格图，使用二元贝叶斯滤波器更新存在概率(1)。此外，未观测到的单元也利用卡尔曼滤波器()2的系统噪声一样随时间衰减 。

（Class）OccupancyGridMap

这个类是继承自nav2_costmap_2d::Costmap2D，就是表示最后对应的占据栅格地图的类

（Class Constructor）OccupancyGridMap::OccupancyGridMap

没什么好说的，根据输入的参数其实底层还是对Costmap2D进行了初始化

（mFunc）OccupancyGridMap::worldToMap

把世界坐标系下的xy转换到了网格中对应的索引

mx = static_cast<int>(std::floor((wx - origin_x_) / resolution_));
my = static_cast<int>(std::floor((wy - origin_y_) / resolution_));

（mFunc）OccupancyGridMap::updateOrigin

这个函数的作用是，在更改网格的原点后对网格整体进行一个更新，首先我们根据给定的新的原点，来找到新的网格和当前网格重叠的部分

// we need to compute the overlap of the new and existing windows
int lower_left_x{std::min(std::max(cell_ox, 0), size_x)};
int lower_left_y{std::min(std::max(cell_oy, 0), size_y)};
int upper_right_x{std::min(std::max(cell_ox + size_x, 0), size_x)};
int upper_right_y{std::min(std::max(cell_oy + size_y, 0), size_y)};

然后这里的local_map的作用就是临时存储窗口中的障碍物信息。然后，将现有的占用网格地图中的窗口区域复制到local_map中。

// we need a map to store the obstacles in the window temporarily
unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
// copy the local window in the costmap to the local map
copyMapRegion(
  costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x,
  cell_size_y);

接下来将重置地图，更新原点的世界坐标为new_grid_ox和new_grid_oy。通过计算的出复制数据回占用网格地图的起始单元格位置start_x和start_y，然后将local_map中的数据复制回占用网格地图的新位置。

// compute the starting cell location for copying data back in
int start_x{lower_left_x - cell_ox};
int start_y{lower_left_y - cell_oy};
// now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(
  local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);

（mFunc）OccupancyGridMap::raytrace2D

看起来是根据机器人位置robot_pose和输入点云pointcloud来生成激光线的部分，里面首先调用raytraceFreespace处理了没有障碍物的空白区域

raytraceFreespace(pointcloud, robot_pose);

之后针对被障碍物填充的地方，在占据栅格地图中的位置标记出来，这里用到MarkCell这个对象，来对障碍物对应的网格索引的位置进行标记

if (!worldToMap(*iter_x, *iter_y, mx, my)) {
  RCLCPP_DEBUG(logger_, "Computing map coords failed");
  continue;
}
const unsigned int index = getIndex(mx, my);
marker(index);

（mFunc）OccupancyGridMap::updateFreespaceCells

更新空白区域，直接调用函数

updateCellsByPointCloud(pointcloud, occupancy_cost_value::FREE_SPACE);

（mFunc）OccupancyGridMap::updateOccupiedCells

这个是更新障碍物栅格地图的

updateCellsByPointCloud(pointcloud, occupancy_cost_value::LETHAL_OBSTACLE);

（mFunc）OccupancyGridMap::updateCellsByPointCloud

这里的更新也是用到了MarkCell，不过这里我有个小小的疑问，因为这里的更新是直接指定了更新的类型，所以输入的话必须也是直接分好的点云吧

MarkCell marker(costmap_, cost);
for (PointCloud2ConstIterator<float> iter_x(pointcloud, "x"), iter_y(pointcloud, "y");
     iter_x != iter_x.end(); ++iter_x, ++iter_y) {
  unsigned int mx{};
  unsigned int my{};
  if (!worldToMap(*iter_x, *iter_y, mx, my)) {
    RCLCPP_DEBUG(logger_, "Computing map coords failed");
    continue;
  }
  const unsigned int index = getIndex(mx, my);
  marker(index);
}

（mFunc）OccupancyGridMap::raytraceFreespace

这里看起来是生成网格中空白区域那种连线的函数，首先是预先计算地图的边界

// we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later
const double origin_x = origin_x_, origin_y = origin_y_;
const double map_end_x = origin_x + size_x_ * resolution_;
const double map_end_y = origin_y + size_y_ * resolution_;

然后是遍历点云中的点，确保当前点是在地图范围内的。最后执行核心函数，调用raytraceLine实现连线

constexpr unsigned int cell_raytrace_range = 10000;  // large number to ignore range threshold
MarkCell marker(costmap_, occupancy_cost_value::FREE_SPACE);
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);

（Class）LaserscanBasedOccupancyGridMapNode

调用laserscan_based_occupancy_grid_map方法所对应的节点类

（Class Constructor）LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode

这里也是加载了很多参数

Name	Type	Description
map_frame	string	map frame
base_link_frame	string	base_link frame
input_obstacle_pointcloud	bool	whether to use the optional obstacle point cloud? If this is true, ~/input/obstacle_pointcloud topics will be received.
input_obstacle_and_raw_pointcloud	bool	whether to use the optional obstacle and raw point cloud? If this is true, ~/input/obstacle_pointcloud and ~/input/raw_pointcloud topics will be received.
use_height_filter	bool	whether to height filter for ~/input/obstacle_pointcloud and ~/input/raw_pointcloud? By default, the height is set to -1~2m.
map_length	double	The length of the map. -100 if it is 50~50[m]
map_resolution	double	The map cell resolution [m]

这里我们可以看到，一定会订阅的是"~/input/laserscan"这个话题，对应的回调函数在onDummyPointCloud2（这个只是把点云转成xyz了应该），然后根据是否有raw_pointcloud和obstacle_pointcloud消息，来判断是否要执行同步回调这个步骤，对应的回调函数又是onLaserscanPointCloud2WithObstacleAndRaw

/* Subscriber and publisher */
laserscan_sub_.subscribe(
  this, "~/input/laserscan", rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile());
obstacle_pointcloud_sub_.subscribe(
  this, "~/input/obstacle_pointcloud",
  rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile());
raw_pointcloud_sub_.subscribe(
  this, "~/input/raw_pointcloud", rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile());
// add dummy callback to enable passthrough filter
laserscan_sub_.registerCallback(
  std::bind(&LaserscanBasedOccupancyGridMapNode::onDummyPointCloud2, this, _1));
if (input_obstacle_and_raw_pointcloud) {
  sync_ptr_ = std::make_shared<Sync>(
    SyncPolicy(5), laserscan_sub_, obstacle_pointcloud_sub_, raw_pointcloud_sub_);
} else if (input_obstacle_pointcloud) {
  sync_ptr_ =
    std::make_shared<Sync>(SyncPolicy(3), laserscan_sub_, obstacle_pointcloud_sub_, passthrough_);
} else {
  sync_ptr_ = std::make_shared<Sync>(SyncPolicy(3), laserscan_sub_, passthrough_, passthrough_);
}
sync_ptr_->registerCallback(std::bind(
  &LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw, this, _1, _2,
  _3));

最后还有个栅格地图更新所对应的类的实例化

（mFunc）LaserscanBasedOccupancyGridMapNode::convertLaserscanToPointCLoud2

把输入的LaserScan转换成PointCloud2类型，这样更方便之后的调用，直接用到了ROS中的库

// convert to pointcloud
PointCloud2::SharedPtr pointcloud_ptr = std::make_shared<PointCloud2>();
pointcloud_ptr->header = laserscan.header;
laserscan2pointcloud_converter_.transformLaserScanToPointCloud(
  laserscan.header.frame_id, laserscan, *pointcloud_ptr, *tf2_);

（mFunc）LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRaw

这个是对应的同步处理的回调函数。这里先把input_laserscan_msg转换成了点云格式，然后对常规的点云数据和障碍物的点云数据根据高度进行了过滤

if (use_height_filter_) {
  constexpr float min_height = -1.0, max_height = 2.0;
  if (!cropPointcloudByHeight(
        *input_obstacle_msg, *tf2_, base_link_frame_, min_height, max_height,
        cropped_obstacle_pc)) {
    return;
  }
  if (!cropPointcloudByHeight(
        *input_raw_msg, *tf2_, base_link_frame_, min_height, max_height, cropped_raw_pc)) {
    return;
  }
}

然后把所有的点云都转换到地图坐标系下，之后就生成了这个单帧的占据栅格地图，并且对空白区域、扫描线和障碍物都进行了对应的更新

// Create single frame occupancy grid map
OccupancyGridMap single_frame_occupancy_grid_map(
  occupancy_grid_map_updater_ptr_->getSizeInCellsX(),
  occupancy_grid_map_updater_ptr_->getSizeInCellsY(),
  occupancy_grid_map_updater_ptr_->getResolution());
single_frame_occupancy_grid_map.updateOrigin(
  pose.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2,
  pose.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2);
single_frame_occupancy_grid_map.updateFreespaceCells(trans_raw_pc);
single_frame_occupancy_grid_map.raytrace2D(trans_laserscan_pc, pose);
single_frame_occupancy_grid_map.updateOccupiedCells(trans_obstacle_pc);

最后是进行发布，如果使用的是单帧模式的话，就直接把当前得到的栅格地图发布出去，不然的话就对occupancy_grid_map_updater_ptr_进行更新，相当于一直在修改占据栅格地图

if (enable_single_frame_mode_) {
  // publish
  occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr(
    map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z,
    single_frame_occupancy_grid_map));
} else {
  // Update with bayes filter
  occupancy_grid_map_updater_ptr_->update(single_frame_occupancy_grid_map);
  // publish
  occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr(
    map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z,
    *occupancy_grid_map_updater_ptr_));
}

pointcloud_based_occupancy_grid_map

这是基于点云的占据栅格地图的生成方法，具体的原理参考官网文档pointcloud based occupancy grid map，这里就不再详细阐述这个原理了

（Class）OccupancyGridMap

这个类和上边的一样，也是继承自nav2_costmap_2d::Costmap2D，不过这里实现的函数不太一样了，下面看一下不太一样的这几个函数

（mFunc）OccupancyGridMap::updateWithPointCloud

通过点云数据更新占用栅格地图。它结合原始点云和障碍点云数据来标记自由空间、未知空间和障碍物。

第一步是初始化角度和角度增量，min_angle, max_angle：表示扫描角度的范围，从 -180 度到 180 度。angle_increment：角度增量，这里设置为 0.1 度。angle_bin_size：计算角度区间的大小，表示在给定范围内的区间数量。然后将点云数据都转换至地图坐标系下。
然后第二步就是创建容器了，并且把点云数据直接按角度分配到各个区间

std::vector</*angle bin*/ std::vector<BinInfo>> obstacle_pointcloud_angle_bins;
std::vector</*angle bin*/ std::vector<BinInfo>> raw_pointcloud_angle_bins;
obstacle_pointcloud_angle_bins.resize(angle_bin_size);
raw_pointcloud_angle_bins.resize(angle_bin_size);
for (PointCloud2ConstIterator<float> iter_x(raw_pointcloud, "x"), iter_y(raw_pointcloud, "y"),
     iter_wx(trans_raw_pointcloud, "x"), iter_wy(trans_raw_pointcloud, "y");
     iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) {
  const double angle = atan2(*iter_y, *iter_x);
  const int angle_bin_index = (angle - min_angle) / angle_increment;
  raw_pointcloud_angle_bins.at(angle_bin_index)
    .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy));
}
for (PointCloud2ConstIterator<float> iter_x(obstacle_pointcloud, "x"),
     iter_y(obstacle_pointcloud, "y"), iter_wx(trans_obstacle_pointcloud, "x"),
     iter_wy(trans_obstacle_pointcloud, "y");
     iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) {
  const double angle = atan2(*iter_y, *iter_x);
  int angle_bin_index = (angle - min_angle) / angle_increment;
  obstacle_pointcloud_angle_bins.at(angle_bin_index)
    .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy));
}

再然后对所有的区间进行排序，排序的依据就是距离range从小到大

// Sort by distance
for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) {
  std::sort(
    obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(),
    [](auto a, auto b) { return a.range < b.range; });
}
for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) {
  std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) {
    return a.range < b.range;
  });
}

接下来先是生成空白区域，就是遍历bin，从机器人到每个bin对应的最远的点的连线

for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) {
  auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index);
  auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index);
  BinInfo end_distance;
  if (raw_pointcloud_angle_bin.empty() && obstacle_pointcloud_angle_bin.empty()) {
    continue;
  } else if (raw_pointcloud_angle_bin.empty()) {
    end_distance = obstacle_pointcloud_angle_bin.back();
  } else if (obstacle_pointcloud_angle_bin.empty()) {
    end_distance = raw_pointcloud_angle_bin.back();
  } else {
    end_distance = obstacle_pointcloud_angle_bin.back().range + distance_margin <
                       raw_pointcloud_angle_bin.back().range
                     ? raw_pointcloud_angle_bin.back()
                     : obstacle_pointcloud_angle_bin.back();
  }
  raytrace(
    robot_pose.position.x, robot_pose.position.y, end_distance.wx, end_distance.wy,
    occupancy_cost_value::FREE_SPACE);
}

接下来标记的是未知的空间，。假设未知单元格位于障碍物后方，因为障碍物后方是一个盲点。因此，未知单元格假定为距离每个障碍物点超过一定距离的单元格。 （这段代码）太长了，就不贴了

最后就是标记障碍空间了，如果障碍物点之间的距离小于或等于distance margin，则填入占用，因为输入可能不准确，障碍物点可能无法确定为障碍物。

（Class Constructor）PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode

Name	Type	Description
map_frame	string	map frame
base_link_frame	string	base_link frame
use_height_filter	bool	whether to height filter for ~/input/obstacle_pointcloud and ~/input/raw_pointcloud? By default, the height is set to -1~2m.
map_length	double	The length of the map. -100 if it is 50~50[m]
map_resolution	double	The map cell resolution [m]

这里只会对raw_pointcloud和obstacle_point两种输入进行同步处理，所以相当于只有onPointcloudWithObstacleAndRaw这一个回调函数。至于这个回调函数和上边的那个也没有太大的区别，最主要的是调用的栅格地图类发生了变化

single_frame_occupancy_grid_map.updateWithPointCloud(filtered_raw_pc, filtered_obstacle_pc, pose);

剩下就没什么好说的了

updater

这个是为了栅格地图更新所写的接口，探究其底层的实现，最终还是基于nav2_costmap_2d::Costmap2D的。然后这里对应的其实是两种方法里的第三步，其中OccupancyGridMapBBFUpdater::applyBBF根据贝叶斯更新公式（Bayes Filter）计算新的栅格值。另一个update使用贝叶斯更新公式更新整个占用栅格地图。

这里的代码其实没有什么好说的，就是对着公式来的。然后这个的用处就是在不进行每帧都发布的时候，对栅格地图进行更新维护

总结

这个部分其实是生成概率栅格地图的，应该在激光SLAM中算是比较重要的部分。针对两种雷达消息分别进行了实现，其中第一种是LaserScans，这个对应的就是类似于扫描线这种消息类型。另一种就是那种很传统的点云格式了。

**#
# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的0度面向前（沿着X轴方向）
#
 
Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 测量的角度间的距离 [弧度]
float32 time_increment   # 测量间的时间 [秒]
float32 scan_time        # 扫描间的时间 [秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 强度数据 [device-specific units]**

总的来说感觉两个办法差别有，但又不是那么大。。