点云同步对于单帧数据无影响，对于多帧数据，延时一帧

1.点云同步处理的类  RangeDataCollator

class RangeDataCollator {
 public:
  explicit RangeDataCollator(
      const std::vector<std::string>& expected_range_sensor_ids)
      : expected_sensor_ids_(expected_range_sensor_ids.begin(),
                             expected_range_sensor_ids.end()) {}
 
  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();
};

其中有四个成员函数：

生成点云的所有传感器的 sensor_ids

  const std::set<std::string> expected_sensor_ids_;
修剪合并点云时的所有点云  sensor_id  和与之对应的数据

  std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
时间撮  记录点云

  common::Time current_start_ = common::Time::min();
  common::Time current_end_ = common::Time::min();

2.AddRangeData

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

1.首先检测该点云数据 sensor_id 是否在期望的sensor_ids里面，否则程序停止

  CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);

2.当该sensor_id 已经在 id_to_pending_data_ 中

当我们有两个相同传感器的消息时，移除两个中旧的数据，但不发送电流

1>跟新current_start,current_end 时间

current_start_ = current_end_;
current_end_ = id_to_pending_data_.at(sensor_id).time;

2>修剪合并在  id_to_pending_data_ 中的数据  CropAndMerge

    auto result = CropAndMerge();

3>将当前传感器数据放入 id_to_pending_data_ 中，并且返回result

    id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
    return result;

3.当sensor_id 不在 id_to_pending_data_ 中时

1>首先当前数据加到 id_to_pending_data_ 中，

  id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);

2>使 id_to_pending_data_ 中 包含期望的所有数据  

  if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
    return {};
  }

3>跟新时间

  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;

4>修剪合并  CropAndMerge

3.CropAndMerge

sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge()

1.定义返回对象

  sensor::TimedPointCloudOriginData result{current_end_, {}, {}};

2.遍历 id_to_pending_data_ 中所有传感器的数据，其对于传感器数据处理一样

  for (auto it = id_to_pending_data_.begin();
       it != id_to_pending_data_.end();) {
    sensor::TimedPointCloudData& data = it->second;
    sensor::TimedPointCloud& ranges = it->second.ranges;

2>将在[current_start_,current_end_]区间内的传感器数据  下表确定出来

    auto overlap_begin = ranges.begin();
    while (overlap_begin < ranges.end() &&
           data.time + common::FromSeconds((*overlap_begin)[3]) <
               current_start_) {
      ++overlap_begin;
    }
    auto overlap_end = overlap_begin;
    while (overlap_end < ranges.end() &&
           data.time + common::FromSeconds((*overlap_end)[3]) <= current_end_) {
      ++overlap_end;
    }

3>复制重叠范围。

    if (overlap_begin < overlap_end) {
      std::size_t origin_index = result.origins.size();
      result.origins.push_back(data.origin);
      double time_correction = 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[3] += time_correction;
        result.ranges.push_back(point);
      }
    }

涉及到：   处理了前一时刻的点云，然后将时间跟新

4>将缓冲点丢弃，直到overlap_end。

    if (overlap_end == ranges.end()) {
      it = id_to_pending_data_.erase(it);
    } else if (overlap_end == ranges.begin()) {
      ++it;
    } else {
      data = sensor::TimedPointCloudData{
          data.time, data.origin,
          sensor::TimedPointCloud(overlap_end, ranges.end())};
      ++it;
    }

3.按照时间排序，并且返回结果

  std::sort(result.ranges.begin(), result.ranges.end(),
            [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
               const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
              return a.point_time[3] < b.point_time[3];
            });
  return result;

感觉cartographer处理点云数据有一帧延时