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) != 0) {
    current_start_ = current_end_;
    // When we have two messages of the same sensor, move forward the older of
    // the two (do not send out current).
    // 采用旧的时间戳
    current_end_ = id_to_pending_data_.at(sensor_id).time;
    auto result = CropAndMerge();
    // 用新的数据替换原有的数据
    id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
    return result;
  }

如果当前融合时间段内融合结果中已包含同一ID的传感器数据，则应采用最新的点云数据进行替换，但是结束的时间戳仍然采用原来的时间刻度，开始进行数据截取合并CropAndMerge()并返回，其CropAndMerge()下部分会详细讲解。简单理解如果有一个传感器频率较高，已经来过一帧并进行了缓存，另外一个未来，这个传感器又来一帧，则先进行截取合并送出结果（实际上就是上帧缓存的点云直接发出），然后将新来的一帧替换换来的buffer。

  // 直接插入
  id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
  // 若现在收到的数据类型未全，即期望收到种类未全，直接退出，无需融合
  if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
    return {};
  }
    current_start_ = current_end_;
  // We have messages from all sensors, move forward to oldest.
  common::Time oldest_timestamp = common::Time::max();
  // 找传感器数据中最早的时间戳
  for (const auto& pair : id_to_pending_data_) {
    oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
  }
  current_end_ = oldest_timestamp;
  return CropAndMerge();

如果来的一个新的id（id_to_pending_data_ buff中未存在）点云数据直接插入即可，如果当前接收到的类型未齐全，则直接退回等待接收新的点云。
如果已经收全，则将最早传感器点云的时间戳，作为终止时间戳；然后进行截取合并CropAndMerge()。
current_end_是下次融合的开始时间，是本次融合的最后时间刻度，但其实是此次融合所有传感器中最早的时间戳

CropAndMerge()

for (auto it = id_to_pending_data_.begin();
       it != id_to_pending_data_.end();)

遍历收齐buff中所有点云。

    sensor::TimedPointCloudData& data = it->second;       // 带原点位置和时间戳
    sensor::TimedPointCloud& ranges = it->second.ranges;  // 仅点云（每一个点也都带有测量时间戳）

    auto overlap_begin = ranges.begin();                  // 记录在所有点云中开始时间戳的位置，即上时刻集合的时间戳
    while (overlap_begin < ranges.end() &&
           data.time + common::FromSeconds((*overlap_begin).time) <
               current_start_) {
      ++overlap_begin;
    }

    auto overlap_end = overlap_begin;                     // 记录所有点云结束时间戳的位置，即此时刻集合的时间戳
    while (overlap_end < ranges.end() &&
           data.time + common::FromSeconds((*overlap_end).time) <=
               current_end_) {
      ++overlap_end;
    }

一个传感点云中的时间戳为点云N个光束的起始时刻，但N个光束采样实际上也存在一定measurement间隔，一般在ros的激光驱动中都已添加，而cartographer中的TimedPointCloud点云类型，是将点云中每个点也都存储了与起始点测量时间间隔的时间戳。通过增加起始时刻的时间戳data.time + common::FromSeconds((*overlap_begin).time) ，则可精确获取每个端点的时间戳。上段代码的目的是此传感器的每个点云需要满足在current_start_和current_end_之间。由于可能存在多个传感器，在融合时，可能会导致前后时间重叠，因此进行时间段限制。

// 如果某个传感器点云前面时间戳早于当前集合定义的时间戳，则丢弃
    if (ranges.begin() < overlap_begin && !warned_for_dropped_points) {
      LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin)
                   << " earlier points.";
      warned_for_dropped_points = true;
    }
    
    // Copy overlapping range.
    if (overlap_begin < overlap_end) {
      std::size_t origin_index = result.origins.size();    //获取下个插入的index，即当前集合的个数
      result.origins.push_back(data.origin);               // 插入原点坐标
      const float time_correction =                        // 获取此传感器时间与集合时间戳的误差，
          static_cast<float>(common::ToSeconds(data.time - current_end_));
      for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
           ++overlap_it) {
        sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it,
                                                                  origin_index};
        // current_end_ + point_time[3]_after == in_timestamp +
        // point_time[3]_before
        point.point_time.time += time_correction;          // 针对每个点时间戳进行修正
        result.ranges.push_back(point);                     
      }
    }

上段代码目的如果未被全部抛弃，则需要将点云每个点的偏移时间进行修正，调整到同一的current_end_为起始时刻（即本次融合统一起始时刻）。

    // Drop buffered points until overlap_end. 
    if (overlap_end == ranges.end()) {
      it = id_to_pending_data_.erase(it);                                     // 每个点都融合进去，则直接清楚此ID的
    } else if (overlap_end == ranges.begin()) {
      ++it;                                                                                     // 完全没融进去，则跳过用于下次融合
    } else {   
      data = sensor::TimedPointCloudData{
          data.time, data.origin,
          sensor::TimedPointCloud(overlap_end, ranges.end())};  //删除点已经融合此次的点
      ++it;
    }

以上是遍历迭代条件，是否删除还是迭代，需要看时间边界。如果点云每个点都考虑进去，则可进行删除。已经考虑

  // 对集合中所有点云，进行按照时间顺序排序
  std::sort(result.ranges.begin(), result.ranges.end(),
            [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
               const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
              return a.point_time.time < b.point_time.time;
            });

融合后，由于均为统一时间戳开始时刻，因此将所有点云按时间排序。如此得到的的结果，如同仅有一个传感器获取的点云。

简单总结:
假设只有一个传感器，每次有一定采样间隔，且新的cloud时间比旧的cloud时间一定更晚，而且晚于前一帧最后一个点的时刻。因此每次点云中所有点均不会删除，且一次性完全会被考虑进去。但由于current_end_为当前帧的起始时刻，而current_start_为上次起始时刻。故此次接收到的点云，不在处理时间边界条件内，则缓存无输出。当再次收到点云帧时，则缓存中的点云在边界范围内，然后在进行重新进行排序（原来本身就是安按顺序排列的），因此可看出只有一个传感器时完全是复制，没有任何变换，但会延时一帧，可看如下示例。但是在实际输出时的点云时间戳全部进行了矫正，用end_作为起始时间（即将延迟的点云帧时间改成较新的时间戳）。
假设仅有sensor1.

从上图时序可看出，当收到第一帧时，由于时间界限为初始时刻到接收此刻间进行输出，显然当前帧不满足，即每次输出都是当前刚刚接收到的数据之前的点云。由于t0时刻为第一帧则无输出将其缓存至buffer中。当接收第二帧时，则处理的第二帧之前的点云，进行输出。由于同一id已有点云，故将新的点云直接替换buffer即可。如此接收第三帧时，送出第二帧，缓存第三帧。

假设两个以上传感器，举例只有两个传感器，较容易理解。其具体过程可以看下如下图。