讲解关于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→官方认证
 

一、前言

在正式讲解之前，回顾一下之前的的博客：
(02)Cartographer源码无死角解析-(22) 传感器数据分发→CollatedTrajectoryBuilder
总的来说，初始注册的回调函数，整理数据之后，最终都通过 CollatedTrajectoryBuilder::AddSensorData 调用CollatedTrajectoryBuilder::AddData() 最后再执行 sensor::Collator::AddTrajectory()，把数据传送给了 GlobalTrajectoryBuilder，但是并没有做具体分析。

在 src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.h 文件中，可以看到看到 CollatedTrajectoryBuilder::AddSensorData() 为重载函数，该重载函数会调用 sensor::MakeDispatchabl() 函数实现模板参数的自动推导。然后把 Dispatchable 类型参数传递给 CollatedTrajectoryBuilder::AddData 函数，其代码如下：

// 将数据传入sensor_collator_的AddSensorData进行排序
void CollatedTrajectoryBuilder::AddData(std::unique_ptr data) {sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}

虽然上一篇博客，对 sensor::Dispatchable 以及其父类 sensor::Data 进行了分析，但是并没有讲述 sensor_collator_->AddSensorData 函数是如何使用这些数据的，也就是说没说明 CollatedTrajectoryBuilder::sensor_collator_的来源与作用

首先根据 CollatedTrajectoryBuilder 构造函数的初始化列表，可知其在创建实例时，已经对 sensor_collator_ 进行了赋值，根据前面的分析，可知 CollatedTrajectoryBuilder 在 src/cartographer/cartographer/mapping/map_builder.cc 文件中的 MapBuilder::AddTrajectoryBuilder() 函数中构建，查看该函数可以找到如下代码：

    trajectory_builders_.push_back(absl::make_unique(trajectory_options, sensor_collator_.get(), trajectory_id,....trajectory_builders_.push_back(absl::make_unique(trajectory_options, sensor_collator_.get(), trajectory_id,....

可以看到 sensor_collator_，易知 sensor_collator_ 为一个智能指针，且是 MapBuilder 的一个成员变量。最中可以在 MapBuilder 的构造函数中找到答案，位于 src/cartographer/cartographer/mapping/map_builder.cc 之中：

  // 在 cartographer/configuration_files/map_builder.lua 中设置// param: MAP_BUILDER.collate_by_trajectory 默认为falseif (options.collate_by_trajectory()) {sensor_collator_ = absl::make_unique();} else {// sensor_collator_初始化, 实际使用这个sensor_collator_ = absl::make_unique();}

本人源码运行使用的是 absl::make_unique()，也就是collate_by_trajectory 使用默认值 false。
 

二、CollatorInterface

sensor::Collator 位于 src/cartographer/cartographer/sensor/internal/collator.h 中实现，其继承于 CollatorInterface。在 CollatorInterface.h 文件中，可以看到类 CollatorInterface 这个接口类与前面的的一些接口类都差不多，主要声明了一些纯虚函数，然后禁用了拷贝构造函数与赋值运算符重载函数。另外可以看到如下比较特别的代码：

  // note: CollatorInterface::Callback 2个参数using Callback =std::function)>;// Adds a trajectory to produce sorted sensor output for. Calls 'callback'// for each collated sensor data.virtual void AddTrajectory(int trajectory_id,const absl::flat_hash_set& expected_sensor_ids,const Callback& callback) = 0;

总的来说，要表的的意思就是 CollatorInterface 的派生类，需要重写 AddTrajectory 函数，该函数无返回值，且需要传递三个参数，第三个参数为用于回调的函数指针，该函数指针无返回值，需要传递两个参数：onst std::string& 与 std::unique_ptr。
 

三、Collator

现在回过头来看 Collator，在 Collator.h 文件中，可以明显的看到其依旧禁用了拷贝构造函数以及赋值运算符重载函数。然后对父类函数进行了重写。值得注意的是，其有两个私有成员变量：

 private:// Queue keys are a pair of trajectory ID and sensor identifier.OrderedMultiQueue queue_; //暂时理解为按时间排序之后的队列即可// Map of trajectory ID to all associated QueueKeys.//key为 trajectory ID，value是与该trajectory ID相关的所有QueueKeysabsl::flat_hash_map> queue_keys_;

不是很明白也没有关系，后续会进行讲解。
 

1、AddTrajectory()

先来看到看其中的 Collator::AddTrajectory 函数，该函数是在 CollatedTrajectoryBuilder 构造函数中被调用的的：

  // sensor::Collator的初始化sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_id_strings,[this](const std::string& sensor_id, std::unique_ptr data) {HandleCollatedSensorData(sensor_id, std::move(data));});

从这里可以看出，CollatedTrajectoryBuilder 在构造函数中，把其成员函数作 HandleCollatedSensorData 作为回调函数传递给了 sensor_collator_->AddTrajectory()。

在前面的接口 CollatorInterface 中提到，其重写 sensor_collator_->AddTrajectory() 时，其第三个参数为函数指针，该函数指针的参数为 std::string& 与 std::unique_ptr 类型，那么可以知道其上的 lambda 表达式刚好满足需求。先来看看代码注释：

/*** @brief 添加轨迹以生成排序的传感器输出, 每个topic设置一个回调函数* * @param[in] trajectory_id 新生成的轨迹的id* @param[in] expected_sensor_ids 需要排序的topic名字的集合* @param[in] callback 2个参数的回调函数, 实际是CollatedTrajectoryBuilder::HandleCollatedSensorData()函数*/
void Collator::AddTrajectory(const int trajectory_id, //轨迹idconst absl::flat_hash_set& expected_sensor_ids, //订阅的话题名字集合const Callback& callback) { //回调函数for (const auto& sensor_id : expected_sensor_ids) { //循环遍历所有话题的名字//为每个话题构建构建一个QueueKey，其包含了trajectory_id与订阅该话题名字const auto queue_key = QueueKey{trajectory_id, sensor_id};queue_.AddQueue(queue_key,//把trajectory_id与题名字共同看作key// void(std::unique_ptr data) 带了个默认参数sensor_id[callback, sensor_id](std::unique_ptr data) {callback(sensor_id, std::move(data));});//把同一trajectory_id的queue_key都存放在一个vector中queue_keys_[trajectory_id].push_back(queue_key);}
}

该函数其实也比较简单，就是为当前 轨迹(trajectory_id) 订阅的每个话题，都构建一个键值对添加到成员变量 OrderedMultiQueue queue_ 中，键值如下：

queue_key = QueueKey{trajectory_id, sensor_id}  //键
[callback, sensor_id](std::unique_ptr data) {callback(sensor_id, std::move(data));} //值

从前面已经知道传入的参数 callback 就是一个 lambda 的类型参数，但是这里又使用了一个 lambda 表达式？这时因为 OrderedMultiQueue queue_ 中存入的 valua 只能虽然时函数指针，但是其只能传入一个参数。最后，键 queue_key 还会被存储在如下变量之中

absl::flat_hash_map> queue_keys_;

其是一个字典，key 就是 trajectory_id，value 为 queue_key = QueueKey{trajectory_id, sensor_id}。后面大家就会知道其目的：

目的：\color{red} 目的：目的： OrderedMultiQueue queue_ 保存了所有轨迹，及轨迹对应所有订阅话题的数据队列，后续详解。
 

2、FinishTrajectory()

// 将 trajectory_id 标记为完成
void Collator::FinishTrajectory(const int trajectory_id) {//传入一个trajectory_id，然后对该id下的td::vector进行遍历for (const auto& queue_key : queue_keys_[trajectory_id]) {//调用MarkQueueAsFinished函数进行处理,把queue_key//对应的队列的变量.finished标志位true,表示该队列以及完成queue_.MarkQueueAsFinished(queue_key);}
}

把一个轨迹中的所有数据队列.finished都标记为true。
 

3、AddSensorData

// 将 trajectory_id 标记为完成
void Collator::FinishTrajectory(const int trajectory_id) {//传入一个trajectory_id，然后对该id下的td::vector进行遍历for (const auto& queue_key : queue_keys_[trajectory_id]) {//调用MarkQueueAsFinished函数进行处理,把queue_key//对应的队列的变量.finished标志位true,表示该队列已经完成queue_.MarkQueueAsFinished(queue_key);}
}

暂时并不知道其具体作用，猜测大概是轨迹完成的时候，可能会调用吧，

4、AddSensorData

该函数就比较熟悉了，实际上就是被 CollatedTrajectoryBuilder::AddData() 调用的 sensor_collator_->AddSensorData() 函数：

// 向数据队列中添加 传感器数据 
void Collator::AddSensorData(const int trajectory_id,std::unique_ptr data) {QueueKey queue_key{trajectory_id, data->GetSensorId()};queue_.Add(std::move(queue_key), std::move(data));
}

根据 trajectory_id 以及数据对应的话题，然后往对应的队列添加数据。

5、剩余函数：

// 将所有数据队列标记为已完成,分派所有剩下的传感器数据
// 只能调用一次, 在 Flush 之后不能再调用 AddSensorData()
void Collator::Flush() { queue_.Flush(); }// 返回在 CollatorInterface 解锁之前需要更多数据的轨迹的 ID
// 对于不等待特定轨迹的实现, 返回 'nullopt'
absl::optional Collator::GetBlockingTrajectoryId() const {return absl::optional(queue_.GetBlocker().trajectory_id);
}

暂时不明白给来干啥的。
 

四、QueueKey

在 src/cartographer/cartographer/sensor/internal/ordered_multi_queue.h 文件中，可以看到如下代码

struct QueueKey {int trajectory_id;      // 轨迹idstd::string sensor_id;  // topic名字// 重载小于运算符, map根据这个规则对QueueKey进行排序// 以tuple规则比较2者, tuple定义了<运算符, 逐个元素进行比较bool operator<(const QueueKey& other) const {return std::forward_as_tuple(trajectory_id, sensor_id)