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

一、前言

再上一篇博客中对 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 extrapolator_; //位姿估计器RangeDataCollator range_data_collator_; //对雷达数据进行时间同步的类

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

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

回答:\color{red}回答:回答: 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点云

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

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

二、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& 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) {}

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

三、RangeDataCollator.h 头文件

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

    // 通过前端进行扫描匹配, 然后返回匹配后的结果std::unique_ptrmatching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);

其实际调用的就是 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;}

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

  explicit RangeDataCollator(const std::vector& expected_range_sensor_ids): expected_sensor_ids_(expected_range_sensor_ids.begin(),expected_range_sensor_ids.end()) {}

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();

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

  const std::set expected_sensor_ids_; //存储不同的 topic name，无重复// Store at most one message for each sensor.std::map id_to_pending_data_; // 待处理的数据common::Time current_start_ = common::Time::min(); //开始时间common::Time current_end_ = common::Time::min(); //结束时间constexpr static float kDefaultIntensityValue = 0.f; //默认点云强度值

下面就来重点分析函数 RangeDataCollator::AddRangeData()。
 

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

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

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

(2):\color{blue}(2):(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);

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

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

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

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

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

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

总结:\color{red}总结:总结: 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();
}

六、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;});

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

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

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

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

其上的 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(),// 将点云从雷达坐标系下转到tracking_frame坐标系系下carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast())} ); // 强度始终为空}

再结合 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 intensities; // 空的
};

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

(03):\color{blue}(03):(03): 通过前面函数 RangeDataCollator::AddRangeData() 的运行，current_start_ 与 current_end_ 已经被赋值。这里需要注意一个点 TimedPointCloudData::time 表示一帧

那么就再所有点云数据中进行查找，找到点云中 最后一个时间戳小于current_start_的点的索引 overlap_begin，以及 最后一个时间戳小于等于current_end_的点的索引