(02)Cartographer源码无死角解析-(29) LocalTrajectoryBuilder2D::AddRangeData()→多雷达数据时间同步

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

再上一篇博客中对 src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc 进行了一个比较粗的讲解，大概的分析了其中的成员函数与成员变量。了解到 GlobalTrajectoryBuilder 主要的功能是依照条件，把数据转发到前后端。

但是有一个重要的函数，那就是 GlobalTrajectoryBuilder::AddSensorData()，该函数并不是简单的把数据发送到前后端，而是直接进行好些复杂的处理，先是进行扫描匹配, 然后将扫描匹配的结果当做节点插入到后端的位姿图中。虽然这里是一句话就描述完了，但是实际的操纵是十分复杂的。

在进入细节分析之前，我们先来看看 LocalTrajectoryBuilder2D 这个类，其头文件路径为 src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h。复杂的先不说，了解其中的几个成员变量，如下:

  ActiveSubmaps2D active_submaps_; //活跃的子图，子图完成了会删除一个，然后再新建一个。
  MotionFilter motion_filter_; //对运动进行过滤，如果运动距离或事件太短，则不进行处理
  scan_matching::RealTimeCorrelativeScanMatcher2D //实时的2D扫描匹配器
  real_time_correlative_scan_matcher_;
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_; //ceres的扫面匹配器 
  std::unique_ptr<PoseExtrapolator> extrapolator_; //位姿估计器
  RangeDataCollator range_data_collator_; //对雷达数据进行时间同步的类
1
2
3
4
5
6
7

剩下的结构体，成员函数等，后面为大家做详细的分析。

$疑问:$ 该篇博客主要讲解点云数据的同步。大家可能存在疑问了，为什么要还要同步。之前在接口函数 CollatedTrajectoryBuilder::AddSensorData() 中，会把数据加入到 Collator::queue_ 这个阻塞队列中，然后按时间分发数据吗？

$回答:$ Collator 最终调用 OrderedMultiQueue::Dispatch() 按时间排序分发数据，但是排序是针对单个话题(传感器)数据进行排序。现在假设有多个相同类型的传感器，这里以两个雷达为例，在一段时间内，其分别获取如下点云数据(按时间排序)：

        --------→  时间戳
雷达一： 1  2  3  4  5  6  7  9  10  11  12  13  14  15 ......1000   //假设共1000点云 
雷达二：	        1  2  3  4  5  6  7  9  10  11  12  13  14  15 ......1000    //假设共1000点云
1
2
3

可以看到，他们的点云数据，存在重叠部分，又因为最终送入到前端的数据，都是基于 tracking_frame = “imu_link” 坐标系的，所以他们重叠部分的数据可以融合，也应该融合在一起进行处理。融合之后数据排列如下：

1  2  3  1  4  2  5  3  6  4  7  5  9  6  10  7  11 ...... 1000 ...... 1000  //共2000点云
1

 

二、LocalTrajectoryBuilder2D 构造函数

该构造函数位于 src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 中实现。

/**
 * @brief 构造函数
 * 
 * @param[in] options //2d轨迹前端相关的配置，主要来自于 src/cartographer/configuration_files/trajectory_builder_2d.lua
 * @param[in] expected_range_sensor_ids 所有range类型的话题(应该是距离传感器类型)
 */
LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
    const proto::LocalTrajectoryBuilderOptions2D& options,
    const std::vector<std::string>& expected_range_sensor_ids)
    : options_(options),//2d轨迹前端的所有配置
      //根据子图的相关配置，构建ActiveSubmaps2D对象
      active_submaps_(options.submaps_options()), 
      //根据运动过滤的配置，构建 MotionFilter 对象 
      motion_filter_(options_.motion_filter_options()), 
      //根据real_time_correlative_scan_matcher配置参数，构建
      //scan_matching::RealTimeCorrelativeScanMatcher2D 相关性扫描匹配类对象
      real_time_correlative_scan_matcher_(
          options_.real_time_correlative_scan_matcher_options()),
      //根据ceres_scan_matcher参数，构建scan_matching::CeresScanMatcher2D对象
      ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
      //根据订阅的话题，构建RangeDataCollator对象，用于对雷达数据进行时间同步的类
      range_data_collator_(expected_range_sensor_ids) {}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

其上的配置参数与选项都来自于 src/cartographer/configuration_files/trajectory_builder_2d.lua 文件。
 

三、RangeDataCollator.h 头文件

现在回过头来看看 GlobalTrajectoryBuilder::AddSensorData() 函数，可见如下代码：

    // 通过前端进行扫描匹配, 然后返回匹配后的结果
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>matching_result = 
    local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);
1
2
3

其实际调用的就是 LocalTrajectoryBuilder2D::AddRangeData() 函数。其主要的功能是处理点云数据, 进行扫描匹配, 将点云写成地图。local_trajectory_builder_->AddRangeData() 函数的实现就比较复杂了，其中包含的东西太多了，不过没有关系，一步一步对齐进行分析即可。先来看其中的第一部分：

  // Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty()) {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }
1
2
3
4
5
6
7

range_data_collator_ 是在 LocalTrajectoryBuilder2D 构造函数，初始化列表中创建，range_data_collator_ 为 RangeDataCollator 的实例对象。先来看看其头文件，可以看到构造函数如下：

  explicit RangeDataCollator(
      const std::vector<std::string>& expected_range_sensor_ids)
      : expected_sensor_ids_(expected_range_sensor_ids.begin(),
                             expected_range_sensor_ids.end()) {}
1
2
3
4

explicit 声明表示禁止该构造函数的隐式转换。注意看，传入的 expected_range_sensor_ids 是一个 string 类型的容器，但是经过初始化列表时候，变成 const std::set 类型的集合。 另外只有两个成员函数：

public:
  // If timed_point_cloud_data has incomplete intensity data, we will fill the
  // missing intensities with kDefaultIntensityValue.
  sensor::TimedPointCloudOriginData AddRangeData(
      const std::string& sensor_id,
      sensor::TimedPointCloudData timed_point_cloud_data);
private:
  sensor::TimedPointCloudOriginData CropAndMerge();
1
2
3
4
5
6
7
8

剩下一些成员变量的介绍如下：

  const std::set<std::string> expected_sensor_ids_; //存储不同的 topic name，无重复
  // 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(); //结束时间

  constexpr static float kDefaultIntensityValue = 0.f; //默认点云强度值
1
2
3
4
5
6
7

其上的 constexpr 表示该静态成员变量必须再类中初始化,下面就来重点分析函数 RangeDataCollator::AddRangeData()。
 

四、RangeDataCollator::AddRangeData() 逻辑i分析

该函数的作用是对多个雷达的数据进行时间的同步，主要步骤如下：

$(1):$ 该函数接收两个参数，第一个参数 sensor_id 表示话题名字，第二个参数 timed_point_cloud_data 表示带时间的点云数据 。这里需要注意到点云数据没有使用引用的方式传递，前面都是以引用的方式进行传递的，所以这里进行了第一次点云数据的拷贝。

$(2):$传递到该函数的点云数据，也就是 timed_point_cloud_data 其是不包含强度信息了。那么是从什么时候起，没有强度信息的呢？在 SensorBridge::HandleLaserScan() 中，其会对点云数据进行分段，后续处理的数据都是分段之后的数据(如果num_subdivisions_per_laser_scan=1,则把所有数据看成一段)。但是在分段时，执行了如下代码：

    carto::sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
1
2

可以看到，其构建分段数据 subdivision 的时候，并没有传入 points.intensities 强度信息，只传入的 points.points，其包含了点云数据与强度，但是没有没有强度信息。所以在这个位置，就丢失了点云强度信息。

$(3):$ 所以在 RangeDataCollator::AddRangeData() 函数中，其 timed_point_cloud_data.intensities 变量是空的。所以执行了如下代码：

  timed_point_cloud_data.intensities.resize(timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
1

把点云数据的强度全部设置为0

$(4):$ 通过 id_to_pending_data_ 变量判断一下相同话题的数据是否存在没有处理完的点云数据，如果有，则优先对之前的点云数据进行处理。其通过调用 RangeDataCollator::CropAndMerge() 函数进行处理，然后把当前的点云数据 timed_point_cloud_data 存储在 id_to_pending_data_ 变量中，然后返回。还需要注意变量：current_start_(上一次时间同步的结束时间) 与 current_end_(本次时间同步的开始时间)，其中 current_end_ 为本次时间同步的结束时间为这帧点云数据的结束时间，即为 TimedPointCloudData::time 参数。

$(5):$ 如果该话题之前没有数据保存在 id_to_pending_data_ 之中，则等待range数据的话题都到来之后再进行处理。同样将 current_start_ 设置为上一次同步结束的时间，然后进行循环查找，找到 所有传感器数据中最早的时间戳(点云最后一个点的时间)，然后赋值给 current_end_，最后调用 CropAndMerge() 函数处理当前点云数据后返回。

$总结:$ RangeDataCollator::AddRangeData() 函数，主要就是获得一段点云的起始时间current_start_与结束时间 current_end_，然后调用 RangeDataCollator::CropAndMerge() 函数进行处理。
 

五、RangeDataCollator::AddRangeData() 代码注释

/**
 * @brief 多个雷达数据的时间同步
 * 
 * @param[in] sensor_id 雷达数据的话题
 * @param[in] timed_point_cloud_data 雷达数据
 * @return sensor::TimedPointCloudOriginData 根据时间处理之后的数据
 */
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
    const std::string& sensor_id,
    sensor::TimedPointCloudData timed_point_cloud_data) { // 第一次拷贝
  CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);

  // 从sensor_bridge传过来的数据的intensities为空
  timed_point_cloud_data.intensities.resize(
      timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);

  // TODO(gaschler): These two cases can probably be one.
  // 如果同话题的点云, 还有没处理的, 就先处同步没处理的点云, 将当前点云保存
  if (id_to_pending_data_.count(sensor_id) != 0) {
    // current_end_为上一次时间同步的结束时间
    // current_start_为本次时间同步的开始时间
    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, std::move(timed_point_cloud_data));
    return result;
  }

  // 先将当前点云添加到 等待时间同步的map中
  id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));

  // 等到range数据的话题都到来之后再进行处理
  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_是本次时间同步的结束时间
  // 是待时间同步map中的 所有点云中最早的时间戳
  current_end_ = oldest_timestamp;
  return CropAndMerge();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

$注意:$ 虽然 current_start_，会被赋值为上一次结束的时间

 

六、RangeDataCollator::CropAndMerge() 逻辑讲解

可以看到 RangeDataCollator::AddRangeData() 的核心是 CropAndMerge() 函数，其会直接对点云进行同步处理。为了方便理解，先先来看该函数的最后一段代码：

  // 对各传感器的点云 按照每个点的时间从小到大进行排序
  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;
            });
1
2
3
4
5
6

代码比较简单，就是说有的点云数据，都按照从小到大的方式进行排序。其复杂的的是 result.ranges 应该如何构建。那么下面我们就来看看。

$(01):$ 首先创建一个 sensor::TimedPointCloudOriginData 结构体 result，结构体的定义后面再进行总结。

$(02):$ 启动第一层遍历，也就是对话题的遍历，获得当前遍历话题点云数据的如下信息：

	 //雷达数据的总体信息，含点云最后一个点时间
    sensor::TimedPointCloudData& data = it->second; 
    const sensor::TimedPointCloud& ranges = it->second.ranges;
    const std::vector<float>& intensities = it->second.intensities;
1
2
3
4

其上的 sensor::TimedPointCloudData，不知道大家是否又印象，之前讲解过的 SensorBridge::HandleRangefinder() 函数，其发送的数据类型就是该类型，代码如下：

 // 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin
  // 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_
  if (sensor_to_tracking != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, 
                       sensor_to_tracking->translation().cast<float>(),
                       // 将点云从雷达坐标系下转到tracking_frame坐标系系下
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())} ); // 强度始终为空
  }
1
2
3
4
5
6
7
8
9
10
11

再结合 TimedPointCloudData 的定义：

// 时间同步前的点云
struct TimedPointCloudData {
  common::Time time;        // 点云最后一个点的时间
  Eigen::Vector3f origin;   // 雷达传感器坐标系到tracking_frame = "imu_link" 坐标系的平移
  TimedPointCloud ranges;   // 数据点的集合, 每个数据点包含xyz与time, time是负的
  // 'intensities' has to be same size as 'ranges', or empty.
  std::vector<float> intensities; // 空的
};
1
2
3
4
5
6
7
8

因为函数回调 SensorBridge::HandleRangefinder() 中，构建 TimedPointCloudData 结构体实例时只对 common::Time time、Eigen::Vector3f origin、TimedPointCloud ranges 进行初始化，所以 std::vector intensities 默认情况下时空的。

$(03):$ 通过前面函数 RangeDataCollator::AddRangeData() 的运行，current_start_ 与 current_end_ 已经被赋值。这里需要注意一个点 TimedPointCloudData::time 表示一帧最后点云时间戳，其为正值，但是 TimedPointCloudData::points::time 是相对于最后点云的时间，通常都负值，当然，如果一帧点云数据几乎同时出来，就是所有点云距离最后一个点云生产的时间间隔太近了，那么就会为 0。

$(04):$ 对所有点云进行遍历，每个点云的时间戳为 TimedPointCloudData::time 再加上其相对于该时间戳的时间，也就是 TimedPointCloudData::points::time，得到该点云的时间戳。找到点云中 最后一个时间戳小于current_start_的点迭代器 overlap_begin。同理找到点云中 最后一个时间戳小于等于current_end_的的点迭代器 overlap_end。

$(05):$ 如果 ranges.begin() < overlap_begin 说明来自同一雷达的点云数据有重叠(点云数据时间戳不规范)，同时 warned_for_dropped_points 又设置为 false 则会进行警告打印，类似如下：

"Dropped 5 earlier points.";  //告知丢失了多少个点云数据，
1

该 warned_for_dropped_points 为一个标志位，每执行一次 CropAndMerge() 只打印一次log

$(06):$ 如果 overlap_begin < overlap_end 成立，说明有点云数据需要进行同步处理。首先获得雷达传感器原点再坐标系tracking_frame = “imu_link” 平移位置，也就是 data.origin，存储在 result.origins 之中，总得来说，result.origins 的就是话题数据对应的origin，并且同时获得了其在 result.origins 中的索引 origin_index，time_correction 记录点云数据与集合时间戳的误差。

$(07):$ 让 intensities 的迭代器 intensities_overlap_it 也指向 与 overlap_begin 相同的位置，也就是说此时 intensities_overlap_it 与 overlap_begin 已经一一对应。为 result.ranges 预留空间，会将之前的数据拷贝到新的内存中。

$(08):$ 进入循环迭代，从 overlap_begin 开始到 overlap_end结束，每次迭代 overlap_it 与 intensities_overlap_it 都会指向下一次。首先其会构建一个 point，其类型为 sensor::TimedPointCloudOriginData::RangeMeasurement。注意 TimedPointCloudData::ranges 就是存储该类实例的容器。TimedPointCloudOriginData 与 RangeMeasurement 的定义如下：

// 时间同步后的点云
struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;   // 带时间戳的单个数据点的坐标 xyz
    float intensity;                    // 强度值
    size_t origin_index;                // 属于第几个origins的点
  };
  common::Time time;                    // 点云的时间
  std::vector<Eigen::Vector3f> origins; // 所有雷达传感器相对于tracking_frame坐标系的位置 
  std::vector<RangeMeasurement> ranges; // 数据点的集合
};
1
2
3
4
5
6
7
8
9
10
11

构建的point先对其做一个时间的矫正，针对每个点时间戳进行修正, 让最后一个点的时间为0。然后添加到 result.ranges 之中。

$(09):$ ①如果遍历完所有需要处理的点云之后 overlap_end == ranges.end()，说明点云每个点都用了, 则可将这个数据 data 从 id_to_pending_data_ 进行删除。②如果一个点都没用, 就先放在id_to_pending_data_中, 看下一个数据。

$(10):$ 如果用了一部分，将用了的点删除, 这里使用的方式是直接对 data 进行赋值替换成没有处理的点云，先当于把用了的从 id_to_pending_data_ 中删除了。最后就是对 result 点云数据进行一个时间的排序。

$如果没有看得很明白，没有关系，继续往下，后面有画图讲解$

七、RangeDataCollator::CropAndMerge() 代码注释

// 对时间段内的数据进行截取与合并, 返回时间同步后的点云
sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
  sensor::TimedPointCloudOriginData result{current_end_, {}, {}};
  bool warned_for_dropped_points = false;
  // 遍历所有的传感器话题
  for (auto it = id_to_pending_data_.begin();
       it != id_to_pending_data_.end();) {
    // 获取数据的引用
    sensor::TimedPointCloudData& data = it->second;
    const sensor::TimedPointCloud& ranges = it->second.ranges;
    const std::vector<float>& intensities = it->second.intensities;

    // 找到点云中 最后一个时间戳小于current_start_的点的索引
    auto overlap_begin = ranges.begin();
    while (overlap_begin < ranges.end() &&
           data.time + common::FromSeconds((*overlap_begin).time) <
               current_start_) {
      ++overlap_begin;
    }

    // 找到点云中 最后一个时间戳小于等于current_end_的点的索引
    auto overlap_end = overlap_begin;
    while (overlap_end < ranges.end() &&
           data.time + common::FromSeconds((*overlap_end).time) <=
               current_end_) {
      ++overlap_end;
    }

    // 丢弃点云中时间比起始时间早的点, 每执行一下CropAndMerge()打印一次log
    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) {
      // 获取下个点云的index, 即当前vector的个数
      std::size_t origin_index = result.origins.size();
      result.origins.push_back(data.origin);  // 插入原点坐标

      // 获取此传感器时间与集合时间戳的误差, 
      const float time_correction =
          static_cast<float>(common::ToSeconds(data.time - current_end_));

      auto intensities_overlap_it =
          intensities.begin() + (overlap_begin - ranges.begin());
      // reserve() 在预留空间改变时, 会将之前的数据拷贝到新的内存中
      result.ranges.reserve(result.ranges.size() +
                            std::distance(overlap_begin, overlap_end));
      
      // 填充数据
      for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
           ++overlap_it, ++intensities_overlap_it) {
        sensor::TimedPointCloudOriginData::RangeMeasurement point{
            *overlap_it, *intensities_overlap_it, origin_index};
        // current_end_ + point_time[3]_after == in_timestamp +
        // point_time[3]_before
        // 针对每个点时间戳进行修正, 让最后一个点的时间为0
        point.point_time.time += time_correction;  
        result.ranges.push_back(point);
      } // end for
    } // end if

    // Drop buffered points until overlap_end.
    // 如果点云每个点都用了, 则可将这个数据进行删除
    if (overlap_end == ranges.end()) {
      it = id_to_pending_data_.erase(it);
    } 
    // 如果一个点都没用, 就先放这, 看下一个数据
    else if (overlap_end == ranges.begin()) {
      ++it;
    } 
    // 用了一部分的点
    else {
      const auto intensities_overlap_end =
          intensities.begin() + (overlap_end - ranges.begin());
      // 将用了的点删除, 这里的赋值是拷贝
      data = sensor::TimedPointCloudData{
          data.time, data.origin,
          sensor::TimedPointCloud(overlap_end, ranges.end()),
          std::vector<float>(intensities_overlap_end, intensities.end())};
      ++it;
    }
  } // end for

  // 对各传感器的点云 按照每个点的时间从小到大进行排序
  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;
            });
  return result;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

 

八、RangeDataCollator 图解汇总

首先为了方便理解，本人绘画了下图(假设有两个雷达)，从 RangeDataCollator::AddRangeData() 函数开始i分析。

$(01):$ 假设现在雷达一(scan_1) 第一次执行 RangeDataCollator::AddRangeData() 函数，那么 id_to_pending_data_ 为空，则把 scan_1 的第一帧点云数据直接添加到 id_to_pending_data_ 中。然后判断
scan_1、scan_2 数据是否都到来，显然没有没有，因为雷达二还没有执行，所以 return。

$(02):$ 现假设第二个雷达(scan_2)订阅话题数据第一帧到来，注意此时 scan_2 也是第一次调用 RangeDataCollator::AddRangeData() 函数。虽然 id_to_pending_data_ 中存储了 scan_1 中的第一帧数据，但是没有存储 scan_2的数据，所以依旧不满足条件 id_to_pending_data_.count(sensor_id) != 0。

$(03):$ 将 scan_2 的数据也添加到 id_to_pending_data_ 之中，也就是说此时 id_to_pending_data_ 包含了 scan_1 与 scan_2的数据。也就是 expected_sensor_ids_ 的话题已经全部到齐。那么把 current_end_ 赋值 给 current_start_ (由于这是第一次赋值，current_end_ 与 current_start_ 都是相同的，为最小时间点，也就是比上图的 $sc1\_st1$ 还要早)，id_to_pending_data_ 中已经存储了 scan_1 与 scan_2 各一帧数据，则找到他们之中之间最早时间戳，我们这里的例子当然就是 scan_1 第一帧点云的时间戳，需要注意的是，这里说的时间戳，是点云帧数据最后一个点的时间戳，对应与上图中的 $sc1\_ed1$。然后把其赋值给 current_end_。

$(04):$ 第一次调用 CropAndMerge() 函数进行数据同步，遍历所有的传感器话题。

$(05):$ 假设现在首先遍历到 secan_1：
① 则获得 secan_1 第一帧总体数据记为data，点云数据记为 ranges，强度记为 intensities。
②对点云数据ranges进行遍历，找到比 current_start_ 还要小的最后一个点云时间戳。显然是找不到的，如上图，因为 $sc1\_st1$ 已经大于 current_start_ 了，也就是该帧点云数据中，最早的点云数据，都是迟于 current_start_ 的，虽然找不到，但是迭代器指向了 ranges.begin()，也就是 $sc1\_ed1$
③对点云数据ranges进行遍历，找到点云中 最后一个时间戳小于等于current_end_的点的迭代器，这个呢还是可以找的的，此时的 current_end_ 为上图的 $sc1\_ed1$，也就是说，找到了最后一个点云数据。
④如果overlap_begin < overlap_end，说明找到了数据。该例子中，到这里肯定是成立的，实际上 overlap_begin 到 overlap_end 目前刚好就上图中 $sc1\_st1$ 到 $sc1\_ed1$ 也就是 secan_1 第一帧数据。这写数据都会被插入到变量：

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

$(06):$ 假设现在首先遍历到 secan_2，因为 secan_1 的第一帧数据已经添加到 result 中，同理会其会把 secan_2 的第一帧数据也添加到 result 之中，但是需要注意的是，这里不会全部添加，如上图所示，只会添加 $sc2\_st1$ 到 $sc1\_ed1$ 的数据。

$(07):$ 把添加到 result 中的数据，都从 id_to_pending_data_ 中删除。也就说，id_to_pending_data_ 中目前只剩下上图中黑色字体【①剩余未处理】时间段的数据。然后对数据进行排序，最终排序之后的 result 包含的数据为 【 $雷达一:sc1\_st1-sc1\_ed1$】与【 $雷达二:sc2\_st1-sc1\_ed1$】的数据，然后返回。

$(08):$ 记住一个点，那就是目前的 $sc1\_ed1$ 时间戳就是代码中的 current_end_。

$(09):$ 现在假设 雷达一secan_1 的第二帧数据来了，即 雷达一secan_1 第二次执行 RangeDataCollator::AddRangeData() 函数，此时进来，其 id_to_pending_data_ 中是没有 secan_1 的数据的，其只有 secan_2 的数据，所以 id_to_pending_data_.count(sensor_id) != 0 不成立。那么 secan_1 把第二帧数据添加到 id_to_pending_data_ 之中。

$(10):$ 那么 id_to_pending_data_ 包含了 expected_sensor_ids_ 的所有数据(secan_1与secan_2) ，虽然 secan_2 只有上次剩余的一小部分。此时把 $sc1\_ed1$ = current_end_ 赋值给 current_start_，然后后又把 current_end_ 设置为 $sc1\_ed2$。然后调用 CropAndMerge()。

$(11):$ 根据前面的分析 ，此次调用 RangeDataCollator::CropAndMerge() 获得的是 【 $雷达二:sc1\_ed1-sc2\_ed1$】与 【 $雷达一:sc1\_ed1-sc1\_ed2$】排序之后的合成数。

$(12):$ 在(09)的假设是 雷达一secan_1 的第二帧数据来了，但是也有可能是 雷达一secan_2 的第二帧数据来了，此时满足条件 id_to_pending_data_.count(sensor_id) != 0。那么把 $sc1\_ed1$ = current_end_ 赋值给 current_start_，然后后又把 current_end_ 设置为 $sc2\_ed1$ ，进行处理，简单的说，就是 雷达一 的数据还来，雷达二的数据就来了，说明雷达二 的数据来的快，那么就赶紧把上次雷达二剩下的数据处理掉，然后在把现在雷达二的数据添加到 id_to_pending_data_ 之中，最后返回结果。这部分内容没有在图上体现。
 

九、结语

其中在 RangeDataCollator::CropAndMerge() 函数中，有个点没有详细讲解，看到如下函数:

    // 丢弃点云中时间比起始时间早的点, 每执行一下CropAndMerge()打印一次log
    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;
    }
1
2
3
4
5
6

该情况只有在同一传感器发送给来的数据，存在重叠时间戳部分，一般来说是不会这样的，具体什么情况下会发生本人暂时也不太清楚，如果后续遇到了会补上。或者知道的朋友也可以告诉我，感激不尽。