(02)Cartographer源码无死角解析-(47) 2D栅格地图→总体流程梳理与总结，及核心关键CastRays()讲解

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

上一篇博客中，对RayToPixelMask()与贝汉明(Bresenham)算法进行了详细的推导，RayToPixelMask() 是被 probability_grid_range_data_inserter_2d.cc 文件中的的 CastRays() 函数调用的，该函数的就根据形参 range_data(存储多个点云数据)，计算每个由雷达传感器原点与所有点云连线所经过的栅格进行更新，如下图所示：

在这里插入图片描述
计算连线经过的栅格主要通过上一博客讲解的 RayToPixelMask() 函数，对于栅格的更新使用 ProbabilityGrid::ApplyLookupTable() 函数。上图红色表示扫描过的区域，灰色表示目前帧在扫描的区域，需要注意的是，同一块区域被重复扫描多次，其地图对应的栅格值则会被更新多次。

该篇博客首先会对 CastRays() 函数进行讲解，然后会对前面的内容做一个总结，也就是梳理一下2D栅格地图的总体流。在讲解 CastRays() 函数之前，先来看看同样位于 probability_grid_range_data_inserter_2d.cc 文件中的 GrowAsNeeded() 函数。

二、核心关键CastRays()

1、GrowAsNeeded()

// 根据点云的bounding box, 看是否需要对地图进行扩张
void GrowAsNeeded(const sensor::RangeData& range_data,
                  ProbabilityGrid* const probability_grid) {
    
    
  // 找到点云的bounding_box，获得目前探索过的区域
  Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());

  // Padding around bounding box to avoid numerical issues at cell boundaries.
  //对区域进行填充，保证雷达点云数据的点都位于该区域类
  constexpr float kPadding = 1e-6f;
  for (const sensor::RangefinderPoint& hit : range_data.returns) {
    
    
    bounding_box.extend(hit.position.head<2>());
  }
  for (const sensor::RangefinderPoint& miss : range_data.misses) {
    
    
    bounding_box.extend(miss.position.head<2>());
  }
  // 对地图进行扩充，新扩充的点云数据对应的cekk坐标被包含在扩充之后的地图之中
  probability_grid->GrowLimits(bounding_box.min() -
                               kPadding * Eigen::Vector2f::Ones());
  probability_grid->GrowLimits(bounding_box.max() +
                               kPadding * Eigen::Vector2f::Ones());
}

通过前面的分析，该部分的代码现在看起来可以直接说是小儿科了，所以这里就不做没有必要的讲解了。

2、CastRays()

/**
 * @brief 根据雷达点对栅格地图进行更新
 * 
 * @param[in] range_data 
 * @param[in] hit_table 更新占用栅格时的查找表
 * @param[in] miss_table 更新空闲栅格时的查找表
 * @param[in] insert_free_space 
 * @param[in] probability_grid 栅格地图
 */
void CastRays(const sensor::RangeData& range_data,
              const std::vector<uint16>& hit_table,
              const std::vector<uint16>& miss_table,
              const bool insert_free_space, ProbabilityGrid* probability_grid) {
    
    
  // 根据雷达数据调整地图范围，保证点云都在地图之内
  GrowAsNeeded(range_data, probability_grid);

  //获得地图的相关信息，如最大最小范围，栅格数，分辨率等
  const MapLimits& limits = probability_grid->limits();
  //获得subpixel系的分辨率
  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
  //新建subpixel系的地图限制
  const MapLimits superscaled_limits(
      superscaled_resolution, limits.max(),
      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                 limits.cell_limits().num_y_cells * kSubpixelScale));
  // 雷达原点在地图中的像素坐标, 作为画线的起始坐标
  const Eigen::Array2i begin =
      superscaled_limits.GetCellIndex(range_data.origin.head<2>());

  // Compute and add the end points.
  //把每个点云数据对应的像素坐标作为结束点，这里只对hit点进行了栅格更新。
  std::vector<Eigen::Array2i> ends;
  ends.reserve(range_data.returns.size());
  for (const sensor::RangefinderPoint& hit : range_data.returns) {
    
    
    // 计算hit点在地图中的像素坐标, 作为画线的终止点坐标
    ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));
    // 更新hit点的栅格值
    probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);
  }
  
  // 如果不插入free空间就可以结束了，也就是不对miss区域进行栅格更新
  if (!insert_free_space) {
    
    
    return;
  }

  // Now add the misses，把前面hit线段经过的栅格进行更新。
  for (const Eigen::Array2i& end : ends) {
    
    
    std::vector<Eigen::Array2i> ray =
        RayToPixelMask(begin, end, kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
    
    
      // 从起点到end点之前, 更新miss点的栅格值
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }

  // Finally, compute and add empty rays based on misses in the range data.
  // range_data 中包含了两种点云数据：RangeData::PointCloud.returns  RangeData::PointCloud.misses
  // RangeData::PointCloud.returns 就是hit点，
  // RangeData::PointCloud.misses 前面是表示雷达没有打中障碍物的点，或者说超出了测量测量范围的数据。然后使用一个值进行代替的数据
  for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {
    
    
    //求得线段经过的栅格
    std::vector<Eigen::Array2i> ray = RayToPixelMask(
        begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),
        kSubpixelScale);
    for (const Eigen::Array2i& cell_index : ray) {
    
    
      // 从起点到misses点之前, 更新miss点的栅格值
      probability_grid->ApplyLookupTable(cell_index, miss_table);
    }
  }
}

该函数也比较简单，需要注意一点的是 const sensor::RangeData& range_data 中的 PointCloud misses 数据，该些点云都是没有打中障碍物或者说超出了测量测量范围的数据。

三、流程梳理总结

下面来梳理一下2D栅格地图构建过程，以及地图的发布与使用。

1、构建过程

对于构建过程，首先来看一下关于栅格相关类的构建过程过程：
$\color{blue}(01)$ LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D() 构造函数，该构造函数会对于成员变量 ActiveSubmaps2D active_submaps_; 进行初始化。

$\color{blue}(02)$ ActiveSubmaps2D::ActiveSubmaps2D 给构造函数，该构造函数会调用成员函数 ActiveSubmaps2D::CreateRangeDataInserter() 对成员变量 std::unique_ptr<RangeDataInserterInterface> range_data_inserter_进行初始化，这里利用了C++多态机制，range_data_inserter_ 实际上是一个 ProbabilityGridRangeDataInserter2D 类型的只能指针。

$\color{blue}(03)$ ActiveSubmaps2D::ActiveSubmaps2D 除了在构造函数中创建 ProbabilityGridRangeDataInserter2D 实例对象，且后续调用 ActiveSubmaps2D::AddSubmap() 函数时，还会构建 Submap2D 对象，添加到其成员变量 submaps_ 之中。

$\color{blue}(04)$ ActiveSubmaps2D::AddSubmap() 函数中构建 Submap2D 对象时，需要给 Submap2D 构造函数传递两个类对象，分别为 MapLimits 与 CellLimits。

$\color{blue}总结$ 以上就是关于2D栅格地图核型类的构建过程。

2、调用过程

LocalTrajectoryBuilder2D::AddRangeData() //处理点云数据
	LocalTrajectoryBuilder2D::AddAccumulatedRangeData() //累加点云数据
		LocalTrajectoryBuilder2D::InsertIntoSubmap() //把点云数据插入子图
			ActiveSubmaps2D::InsertRangeData()//把输插入到活跃的子图
				Submap2D::InsertRangeData()
					ProbabilityGridRangeDataInserter2D::Insert() //输出插入到栅格地图
						//src/cartographer/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.cc
						CastRays()//对栅格地图进行更新
							//src/cartographer/cartographer/mapping/internal/2d/ray_to_pixel_mask.cc
							RayToPixelMask() //核型算法（贝汉明→Bresenham）

其上就是地图更新，相关函数的调用过程。更新的核心算法函数为 RayToPixelMask()。上一篇博客中有进行详细的讲解，且得到了如下一幅图像，对其算法原理进行了形象的描述：
在这里插入图片描述

3、地图保存

目前已经知道了2D栅格地图相关类的构建过程，以及如何点云数据插入且更新栅格地图相关函数的调用过程，下面来看看地图是如何被保存与使用的。回到 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数中，存在如下代码：

	LocalTrajectoryBuilder2D::AddAccumulatedRangeData(){
    
    
		  // 将校正后的雷达数据写入submap
  		std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(time, range_data_in_local, filtered_gravity_aligned_point_cloud,pose_estimate, gravity_alignment.rotation());
  		......
  		  // 返回结果 
	  return absl::make_unique<MatchingResult>(
	      MatchingResult{
    
    time, pose_estimate, std::move(range_data_in_local),
	                     std::move(insertion_result)});
	}

可知插入地图的结果：

  // 将点云插入到地图后的result
  struct InsertionResult {
    
    
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;  // 最多只有2个子图的指针
  };

被包含在结构体 MatchingResult 中返回了。该返回的结果在 src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc 文件的重载函数 AddSensorData() 中被接收，如下所示：

	void AddSensorData(
		const std::string& sensor_id, //订阅的话题
		const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    
    
		......
		    // 通过前端进行扫描匹配, 然后返回匹配后的结果
    	std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data);
		......
		      // 这里的InsertionResult的类型是 TrajectoryBuilderInterface::InsertionResult
      	insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
    
    node_id, 
          matching_result->insertion_result->constant_data,
          std::vector<std::shared_ptr<const Submap>>(
              matching_result->insertion_result->insertion_submaps.begin(),
              matching_result->insertion_result->insertion_submaps.end())});
	    // 将结果数据传入回调函数中, 进行保存
	    if (local_slam_result_callback_) {
    
    
	      local_slam_result_callback_(
	          trajectory_id_, matching_result->time, matching_result->local_pose,
	          std::move(matching_result->range_data_in_local),
	          std::move(insertion_result));
	    }
	}

其上的变量 insertion_result 就是插入点云输入插入地图之后返回的结果。该变量会被回调函数 GlobalTrajectoryBuilder::LocalSlamResultCallback local_slam_result_callback_ 调用。该回调函数实际上就是 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 中的一个 lambda表达式如下：

      [this](const int trajectory_id, 
             const ::cartographer::common::Time time, 
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
    
    
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      }

从而可知道其最终调用到了 MapBuilderBridge::OnLocalSlamResult() 函数。该函数会把这些数据存储在成员变量 local_slam_data_ 中，如下所示：

  std::unordered_map<int,std::shared_ptr<const LocalTrajectoryData::LocalSlamData>> local_slam_data_ GUARDED_BY(mutex_);

如果想获取该数据，需要调用 MapBuilderBridge::GetLocalTrajectoryData() 函数。该函数在 src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 文件中的 void Node::PublishLocalTrajectoryData() 函数中被调用：

  for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
    
    
  	......
  }

也就是说，到此为止就完成了子图话题的发布。

四、结语

通过该篇博客的总结，各位朋友对于 Cartographer 中2D栅格地图的理解应该比较深刻了，后续就是讲解点云扫描匹配了，不用多说，其肯定是 Cartographer 最核型的部分。回到 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数中，可见代码如下：

  // local map frame <- gravity-aligned frame
  // 扫描匹配, 进行点云与submap的匹配
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

该部分的类容，就是后面的核心主题了。