cartographer 代码思想解读（8）- 多激光传感器同步融合RangeDataCollator

前面已经分析了cartographer前端的主要核心算法如插入更新和匹配算法等，其中激光雷达点云数据均为作为主要输入，使用时无需考虑具体几个传感器，传感器类型，可通认为是一个雷达产生的点云数据。但实际cartographer通过RangeDataCollator类将多种传感器进行了融合，并进行了时间同步，最后形成对应的时间戳，pose，和点云集合，在真正使用时通过畸变校准并构建scanmatch和submap插入的传感器格式rangedata。
代码目录如下:
cartographer/mapping/internal/range_data_collator.h

RangeDataCollator类定义

  // 插入集合
  sensor::TimedPointCloudOriginData AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data);

 private:
  // 融合
  sensor::TimedPointCloudOriginData CropAndMerge();
  // 期望处理传感器类型清单
  const std::set<std::string> expected_sensor_ids_;
  // Store at most one message for each sensor.
  // 同步和融合后集合，一种传感器至少一帧点云数据
  std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
  // 时间同步用于时间解析
  common::Time current_start_ = common::Time::min();
  common::Time current_end_ = common::Time::min();

类中包含主要元素融合后的结果输出和初始化配置的期望传感器ID清单。

AddRangeData（）

其所有传感器原始数据进行插入融合的核心方法为ensor::TimedPointCloudOriginData AddRangeData(),将其进行详细分析

 sensor::TimedPointCloudOriginData AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& timed_point_cloud_data);

输入为

1. 传感器类型ID，cartographer将每种传感器都以字符串命名ID；
2. 带时间戳、origin pose的点云数据；

  // 此传感器类型数据已有
  if (id_to_pending_data_.count(sensor_id)