江南才尽江南山，年少无知年少狂！

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

/ Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder Node node(node_options , std :: move(map_builder) , & tf_buffer , FLAGS_collect_metrics);// 如果加载了pbstream文件, 就执行这个函数 if(!} // 使用默认topic 开始轨迹 if(FLAGS_start_trajectory_with_default_topics) {

上一篇博客中，对于 Node::HandleWriteState() 函数进行了分析，其通过调用 MapBuilderBridge::SerializeState() 进而调用到 MapBuilderStub::SerializeStateToFile() 把 MapBuilderBridge 所有成员变量对应的数据都保存下来。

上一篇博客中，讲解了3D点云地图、子图位姿、Landmark 数据消息的发布名，但是并没有对Node::PublishLocalTrajectoryData()、Node::PublishTrajectoryNodeList()、Node::PublishConstraintList() 进行分析，不过需要注意一点。如果使用的是3D轨迹，才会调用 Node::PublishPointCloudMap 发布 3D点云数据的画图。

通过前面一系列博客的分析，到目前为止，node.cc 文件中有关于数据发布的函数，只有 Node::PublishConstraintList() 函数没有讲解了。// 每0.5s发布一次约束数据 void Node :: PublishConstraintList(const :: ros :: WallTimerEvent & unused_timer_event) {} }

通过前面的博客，到目前为止，关于 Cartographer 数据处理，前端，后端都讲解完成了，也就是说，目前已经能够获得比较准确的地图以及位姿了。那么这些位姿外界 cartographer_ros 是如何获取到的，且进行可视化发布到 rviz 的呢？接下来，我们会从 src/cartographer 跳出来，回到最初的 cartographer_ros 部分继续分析源码。

个人理解个人理解这里，本人抛砖引为引玉，谈一下个人对后端优化的理解。首先后端优化是从一个整理来考虑的，其基于 global 系。利用到到子图与子图之间的变换关系，可以在前端(基于local系)过程中，子图单个相对于 local系 的位姿，可以说他们是没有直接联系到一起的。除了子图，节点也一样，前端的节点都是根据活跃子图计算出相对于 local系 的位姿，但是却没有和前面已经完成的子图联系到一起，也显得比较孤立。

核心理解核心理解在上一篇博客中介绍的约束残差(节点相对与子图位姿的残差)，其主是优化每个节点以及每个子图的位姿。可想而知，其是离散的。但是轨迹是一条连续平滑的曲线，所以本质上我们希望通过插值获取到的位姿也是准确的。恰好， landmark帧的时间点，也激光雷达数据帧的时间点是不一样的，所以如果 landmark帧的时间点位于两个激光雷达数据帧的时间点，那么可以把landmark帧相对于机器人的位姿作为一个约束，对两节点插值之后的位姿进行优化，从而广播到两个节点，间接对两个节点进行优化。

通过上一篇博客的小结，可以知道下面的目标就是对 PoseGraph2D::RunOptimization() 函数进行分析了，该函数是累计计算节点的约束达到一定数量时，才会在 PoseGraph2D::DrainWorkQueue() 函数中被调用。疑问1疑问1global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。

上一篇博客中，分析了PoseGraph2D 与 OptimizationProblem2D 的交互过程。疑问1疑问1global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。也就是说，针对于前面的所有困惑都在源码中找到了答案。当然，此时又出现了新的疑问，那就是 optimization_problem_->Solve() 具体是如何实现优化的。

通过前面的博客，已经对 FastCorrelativeScanMatcher2D -分支定界算法(BranchAndBound) 有了十分深入的了解。疑问1疑问1global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。这两个函数区别，被调方式与区别，如何做一个整体的理解。先来梳理一下，这段时间接触过的代码流程梳理一下。

在上一篇博客中对 PrecomputationGrid 与 PrecomputationGridStack2D进行了纤细分析，其主要作用是用于构建多分辨率低，且在最后我们提到，存储在 PrecomputationGridStack2D 中层级越高的，则表示其分辨率约粗。该篇博客主要的目的就是实现如下：由图1、图2、图3 估算出机器人(节点在) 图gt 的位姿。这里假设 .lua 文件中的参数 branch_and_bound_depth=3为例进行讲解。

在分析源码之前，这里先把前面没有解答的疑问复制一下，免得忘记了疑问1疑问1global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。疑问2疑问2为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。疑问4疑问4ComputeConstraintsForNode() 如果返回需要优化，源码中是在哪里执行优化的呢？

在上一篇博客中，主要对 PoseGraph2D::ComputeConstraint() 函数进行了分析。// 进行局部搜索窗口 的约束计算 (对局部子图进行回环检测) constraint_builder_ . MaybeAddConstraint(submap_id , submap , node_id , constant_data , initial_relative_pose);

上一篇博客中，已经对疑问3\color{red}疑问3疑问3进行了解答，目前还存在如下疑问，并没有在源码中找到答案疑问1疑问1global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。疑问2疑问2为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。疑问4疑问4。

疑问1疑问1global_submap_poses 等价于 PoseGraph2D::data_.global_submap_poses_2d 是何时进行优化的。疑问2疑问2为什么要等待约束计算完成之后再调用 PoseGraph2D::HandleWorkQueue()，同时源码又是如何实现的。疑问3疑问3data_.global_submap_poses_2d 中存储子图的全局位姿，那么子图的全局位姿又是从哪里来的呢？疑问4疑问4。

上一篇博客中，以 DrainWorkQueue()、AddWorkItem() 为例，讲解了线程池的一个应用，或许从这个例子来看，线程池似乎很简单的，其实不然。后续的讲解也是围绕着这两个类，不过 ThreadPool 先对来说比较简单，下面先要分析的是 Task。总的来说 Task 可是说是一个任务调配系统，或者说机制。Cartographer 中很多任务都存在依赖关系，具体例子后续分析源码再讲解。这里列举一个比较简单的示例：比如存在任务2，记为task2341task234。

*** @brief 向节点列表中添加一个新的节点, 并保存新生成的submap* @param[in] constant_data 节点数据的指针* @param[in] trajectory_id 轨迹id* @param[in] insertion_submaps 子地图指针的vector* @param[in] optimized_pose 当前节点在global坐标系下的坐标* @return NodeId 返回新生成的节点id。

上一篇博客，主要是对 src/cartographer/cartographer/mapping/id.h 文件中的 class MapById 进行了分析，主要核心是围绕成员变量 MapById::trajectories_ 的两个迭代器：ConstIterator、ConstTrajectoryIterator。前者能够实现对数据节点的遍历操作，后者是对轨迹的迭代与遍历。这里把上一篇博客绘画的图再贴一下：这样大家会有更加直观的了解。疑问1疑问1。

现在开始，以PoseGraph2D::AddNode()为线索，进行展开，对 PoseGraph2D 中所有函数进行细节分析。// local系下的位姿变换到global系下 const transform :: Rigid3d optimized_pose(GetLocalToGlobalTransform(trajectory_id) * constant_data -> local_pose);

通过上一篇博客，再次对 global_trajectory_builder.cc 文件中的 AddSensorData() 进行了分析，同时简要提及位姿图。在Cartographer 中，位姿图优化核心类为 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc 文件中的 PoseGraph2D。其函数 PoseGraph2D::AddNode() 起到了前端与后端交互作用。

通过前面的一系列博客，已经完成了2D点云扫描匹配：含相关性暴力搜索匹配以及ceres扫描匹配的讲解。总得来说，关于Cartographer前端数据处理以及前端位姿优化的相关代码已经深入解析。那么接下来，就是对后端部分代码讲解，这里暂时还是以2D后端优化为例，后续再向3D扩展。由节点和边组成的一种数据结构, 节点之间的关系可以是任意的, 图中任意两节点之间都可能相关(存在边)。

/ 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准 if(options_ . use_online_correlative_scan_matching()) {也就是暴力扫描匹配，已经进行了细致的分析。

上一篇博客中对类 SearchParameters 进行了详细的介绍，同时对 src/cartographer/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc 文件中的 RealTimeCorrelativeScanMatcher2D::Match() 进行了大致的讲解。下面对 RealTimeCorrelativeScanMatcher2D 的各个函数进行一个具体的分析。

通过前面你的一系列博客，已经知道 Cartographer 中的概率栅格图是如何建立的了。不过需要注意的一点是该地图并不仅仅是保存下来给来看的，其还会被点云扫描匹配使用到，点云扫描匹配目的是估算位姿(该部分内容后面会详细讲解)。在 slam 中分为 Scan match 与 Point cloud match，通常情况下前者指的就是2D扫描匹配，后者指的是3D点云匹配。为了方便后续代码的理解，这里简单介绍一下扫描匹配的原理，先来看下图(理想位姿)：图一：理想位姿1 、方格 → 所有的栅格组成。

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

上一篇博客中，首先介绍了 ValueConversionTables，其最终的目的是生成一个转换表，该转换的主要的功能是把[0,1∽32767][0,1∽32767]的数值映射至[0.9,0.1∽0.9][0.9,0.1∽0.9]。虽然后重点讲解 Grid2D::GrowLimits() 函数，该函数主要功能就是判断传入的 point 是否位目前的子图之中，如果不在，则会把地图扩大至原来的四倍，直到 point 处于子图之中。

在上一篇博客中，对 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件中的类 ActiveSubmaps2D 各个成员函数都进行了介绍，其主要功能如下图所示：通过调用 ActiveSubmaps2D::InsertRangeData() 函数向子图 submaps_ 中插入数据，其会使得两个连续的子图之间的数据存在交集。如上图的子图1与子图2存在交集，同时子图2与子图3也存在交集。

通过前面一系列的博客，PoseExtrapolator 进行了比较细致的分析。到目前为止，对于点云数据的预处理过程可以说时十分了解了，如：点云数据多传感器时间同步、运动畸变校正、重力校正、体素滤波等。做完这一系列的预备工作之后，实际上呢，就可以进行点云的扫描匹配了。在讲解扫描匹配之前，先来看看 Cartographer 2D 的栅格地图，其不像3D点云地图有很多成熟的库可以调用，具有统一的标准。大多数 2D Slam 的栅格地图都是需要自己编写代码进行构建的。

通过前面的博客，对于 PoseExtrapolator 这个类可以说是十分了解了，但是其使用方式与具体作用了解还不够透彻，另外在往推测器中添加各种数据(如点云匹配获得的位姿，Imu传感器数据，里程计odome)时，因该如何取舍，或者说谁为主。首先说其作用，或许有的朋友比较奇怪，既然通过点云匹配、或者Imu传感器与里程计odome能够获取位姿，那么直接使用他们的位姿不是就可以了吗？为什么还要搞个推断器来推测位姿？完全没必要啊！虽然点云匹配、或者Imu传感器与里程计odome能够获取位姿。

void PoseExtrapolator :: AddPose() //添加pose数据 void PoseExtrapolator :: AddImuData() //添加Imu数据 void PoseExtrapolator :: AddOdometryData() //添加里程计数据这个三种数据，都包含姿态与位置信息。首先能够想到的问题是，来自于这么多种不同类型的数据，但是都是表述姿态的信息，应该以那个为助，那个为辅呢？

接下来就是对 PoseExtrapolator 的成员函数进行细致分析了，需要注意的是，对于成员函数的分析，也是有逻辑，有目的的，并不是杂乱无章的分析。//const = 0，表示该函数不改变成员变量且为纯虚函数，子类必须从重写 virtual common :: Time GetLastPoseTime() const = 0;//返回最后一次推断器推断出位姿的时间点 virtual common :: Time GetLastExtrapolatedTime() const = 0;

关于 LocalTrajectoryBuilder2D 有关于点云数据处理得部分已经讲解完成了，但是比较杂，比较乱，因为很多地方可能都是跳着讲解得。为了方便大家的理解，这里把相关的总要环节都复盘一下。首相再重述一下之前已经讲解过的数据系统构建过程。(1)(1)

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

/ Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的 auto synchronized_data = range_data_collator_ . AddRangeData(sensor_id , unsynchronized_data);//判断同步的点云数据是否为空， //通常在多雷达传感器中，expected_sensor_ids_ 数据没有到齐。//表示数据已经添加到数据整理器的缓存中。}

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

通过上一篇博客的复盘，已经可以很清晰的知道数据流动过程，总的来说，会依照时间的先后顺序，把数据传送给 GlobalTrajectoryBuilder。collate_fixed_frame = true , --是否将GPS数据放入阻塞队列中，按时间排序再进行分发 collate_landmarks = false , --是否将landmarks数据放入阻塞队列中，按时间排序再进行分发在对 GlobalTrajectoryBuilder 进行深入分析之前，先来看看其构建过程。

通过前面一系列的博客，相信对于 Cartographer 数据的初步处理已经有了一定了解，接下来会一个总结，把之前的所有知识都串联起来。其主要分二个部分进行讲解：1.回调函数的注册与绑定。2.数据流向与分发。那么下面就开始把通过该篇博客，我们可以说是对 Cartographer 数据的分发十分了解了，知道七传感器的数据，最终都传递给了 GlobalTrajectoryBuilder 进行处理。

通过前面的博客，对于 src/cartographer/cartographer/sensor/internal/ordered_multi_queue.cc 中 OrderedMultiQueue 类的成员函数基本上都进行了详细的讲解。从该类的名字来看，可以知道，这是这个多队列数据排序的类，实际也确实如此。该类的主要功能就按时间戳对数据进行排序。大部分成员函数都在前面讲解了，下面来讲其中最重要的成员函数 OrderedMultiQueue::Dispatch()。

在上一篇文章中，对 src/cartographer/cartographer/sensor/internal/ordered_multi_queue.cc 中的 class OrderedMultiQueue 还有好些函数没有进行讲解。回顾一下 ordered_multi_queue.h 这个头文件，可以看到之前提及到的结构体// 存储数据的队列 Callback callback;// 本数据队列对应的回调函数 bool finished = false;