Cartographer源码解读

Cartographer是google的工程师开发的一个用于室内实时建图的SLAM解决方案。总体上可以看作是由局部地图更新和全局回环检测两个部分构成。在局部地图更新过程中，通过最优位姿估计，把激光扫描数据插入到当前维护的子图(submap)中。由于局部地图只使用最近一段时间的传感器数据，所以存在累积误差的问题。这一问题通过全局的闭环检测来加以修正，这还是一个优化问题，Cartographer通过分支定界的方式提高了算法的运行效率。

算法原理:2016 - Real-time loop closure in 2D LIDAR SLAM - Hess et al.pdf
cartographer官网:https://google-cartographer.readthedocs.io/en/latest/#
Github:https://github.com/googlecartographer/cartographer.git

第一部分：ROS环境封装cartographer_ros

Cartographer本身是一个C++的库，虽然可以不依赖ROS的环境运行，但为了快速的上手，google还是提供了一个ROS环境下的封装cartographer_ros。 ROS对于我们来说并不陌生，在机器人操作系统之ROS中已经介绍了ROS的基本环境和常用工具。还通过Turtlebot的建图导航之旅了解了ROS下常用的建图导航技术栈Navigation。我们的cartographer之旅也将从这个ROS环境开始。

Cartographer的总体框架与安装试用	本文中，我根据自己的理解解释一下官方的系统框图，然后介绍如何快速的安装试用Cartographer，并简单的分析一下系统运行时的节点和主题。
官方的ROS封装——cartographer_ros	本文中我们简略的查看了cartographer_ros的目录结构，分析了示例demo的launch脚本发现实际与定位建图相关的节点只有cartographer_node，然后追踪编译系统了解到cartographer_node的入口main函数定义在node_main.cc中。
系统入口——cartographer_node	我们了解到实际与定位建图相关的节点只有cartographer_node，而其入口main函数则定义在node_main.cc中。通过对该文件的分析，我们猜测对象node实际控制着系统的业务逻辑，而对象map_builder则用于完成建图。
cartographer_node的外墙——node对象	本文中我们研读与node对象相关的源码，分析其构造函数以及订阅传感器消息的机制，浏览node对象的消息回调函数与服务响应函数。将会发现ROS系统与Cartographer内核之间的信息交换基本上都是通过对象map_builder_bridge_来完成的。
通往Cartographer的桥梁——map_builder_bridge_	本文中，我们将要分析map_builder_bridge_对象的构造函数和一些重要的成员变量。该对象通过SensorBridge将ROS系统中的传感器数据转换到Cartographer中。
使用SensorBridge转换激光传感器数据	本文中通过分析SensorBridge处理激光传感器数据的函数，我们发现它先通过函数ToPointCloudWithIntensities将ROS的消息数据转换成Cartographer中的点云数据，然后根据配置将激光的扫描数据分段，并将拆分后的数据喂给Cartographer提供的轨迹构建器trajectory_builder_。
使用SensorBridge转换IMU数据	本文将详细分析ROS系统和Cartographer中IMU数据的表示方式，并研究数据转换函数ToImuData。最后会看到SensorBridge对象直接将转换后的IMU数据喂给了路径跟踪器。

第二部分：地图构建器map_builder

通过对cartographer_ros的分析，可以认为实际完成地图构建的就是在系统运行之初所构建的map_builder对象。它是Catographer的顶层对象，为用户提供了一套统一的接口MapBuilderInterface。封装了对象pose_graph_和trajectory_builders_来分别完成闭环检测和子图构建。还构建了一个具有固定数量的线程池，用于管理多线程。

我们将专门用两个部分来介绍pose_graph_和trajecory_builders_两个对象，来具体了解Cartographer的算法。线程池暂时放在本部分中作为map_builder的一个工具来解释。

初识map_builder	本文中通过对map_builder对象的分析，我们可以看到它用对象pose_graph_在后台完成闭环检测和全局的地图优化，并用trajectory_builders_在前台跟踪运动轨迹完成局部的子图构建。
map_builder的接口实现	本文中我们简单看了一下map_builder对象的接口实现，重点分析了AddTrajectoryBuilder函数，看到实际完成局部建图任务的是一个LocalTrajectoryBuilder2D类型的对象，而与轨迹规划器相关的类型还有CollatedTrajectoryBuilder、GlobalTrajectoryBuilder、TrajectoryBuilderInterface它们应该说是一种封装。
连接前端与后端的桥梁——GlobalTrajectoryBuilder	我们会在GlobalTrajectoryBuilder的成员变量中，看到了前端和后端的核心LocalTrajectoryBuilder2D和PoseGraph2D。也将在它处理点云数据的接口中，看到该类是如何把前端的输出结果喂给后端的。

第三部分：Local SLAM

在第二部分中我们剥去层层封装之后，看到实际完成Local SLAM的扫描匹配、更新子图的功能是在类LocalTrajectoryBuilder2D中完成的。扫描匹配负责根据地图信息和激光扫描数据估计机器人的位姿，然后根据位姿估计将传感器数据插入到子图中。不断的重复这两个过程就可以得到一个较为准确的地图，但是对于比较大的场景每次扫描匹配都对整个地图进行搜索的话，其计算量太大。出于实时性的考虑Cartographer只在一个较小的子图中完成这一更新过程，所以是Local SLAM。在一些资料中被称为前端。

Local SLAM的核心——LocalTrajectoryBuilder2D	本文中，通过对类LocalTrajectoryBuilder2D的分析，大体上能够找到进行Local SLAM的一些核心要素以及实现这些要素的对象。走到这里，我们终于看到了算法的一点影子了。
子图的维护与封装	本文中，我们顺着LocalTrajectoryBuilder2D中维护的子图对象active_submaps_，分析其数据类型ActiveSubmaps2D，以及封装子图的类Submap2D。 ActiveSubmaps2D使用了一种类似双缓存的机制，在旧图上进行扫描匹配，同时孕育着新图。
占用栅格的数据结构	子图也是一种占用栅格形式的地图，本文从ProbabilityGrid开始分析占用栅格的数据结构和接口。将会看到Cartographer通过查表的方式更新栅格单元的占用概率。
查找表与占用栅格更新	本文中我们分析插入器对象range_data_inserter_，介绍栅格单元占用概率的更新原理，并详细分析查找表的构建和使用方法。
Local SLAM的业务主线——AddRangeData	本文中我们将详细分析类LocalTrajectoryBuilder2D的成员函数AddRangeData，以及它直接或者间接调用的函数，具体完成了扫描匹配、更新子图的任务。
基于Ceres库的扫描匹配器	本文我们将简单的介绍使用Ceres库的套路。再分析扫描匹配器CeresScanMatcher2D，研究它是如何评价激光扫描数据与栅格地图的匹配程度的。
基于实时相关性分析的扫描匹配器	这个扫描匹配器的作用是初步优化位姿估计器输出的位姿估计，给Ceres匹配器提供一个较好的迭代初值。由于可以通过配置文件选择关闭它，暂时不详细分析，等整体分析完Cartographer，介绍了闭环检测之后再补上。

第四部分：Global SLAM

Global SLAM在后端为Cartographer提供闭环检测全局优化的功能。它的主要工作有三个方面：(1) 通过闭环检测构建子图与路径节点之间的约束关系，进而描述成一个位姿图。 (2) 然后通过SPA(Sparse Pose Adjustment)在后台进行优化。(3) 由于这个SPA优化是一个很耗时的过程，期间前端可能会产生新的子图和路径节点，所以在完成一次优化之后应当调整新增的路径节点的位姿估计。

2010 - Efficient Sparse Pose Adjustment for 2D Mapping - Konolige et al.pdf

Global SLAM的核心——PoseGraph2D	在Cartographer中的位姿图是由轨迹节点和子图构成的二部图，图中的约束描述的是轨迹节点与子图之间的位置关系。后端优化就是估计轨迹节点与子图在世界坐标系下的位姿，最小化全局估计与局部估计之间的偏差。本文我们将研究一下PoseGraph2D的成员变量，以及与位姿图相关的一些数据结构。
位姿图的创建与更新	本文我们将详细分析位姿图PoseGraph2D的构造与更新过程。将看到PoseGraph2D通过对象constraint_builder_在后台完成闭环检测构建约束。它还有一个工作队列来协调后台的任务。
约束构建器——constraint_builder_	本文中我们将详细分析约束构建器constraint_builder_，将看到它是如何通过MaybeAdd-WhenDone调用循环，借助线程池来组织闭环检测和约束的并行计算。
分支定界闭环检测的原理和实现	Cartographer使用类FastCorrelativeScanMatcher2D具体实现了深度优先的分支定界搜索算法，该算法能够高效地进行扫描匹配，计算路径节点与子图之间的约束关系。
后端优化过程	本文我们将回到Global SLAM的系统框图，来了解经过闭环检测构建了子图与路径节点之间的约束之后，都做了些什么工作。通过简要的分析后端优化求解器的数据结构以及基于Ceres库进行SPA(Sparse Pose Adjustment)的优化方法，来了解后端优化的过程。

第五部分：工具

位姿估计器	简介。
运动滤波器	简介。
地图拼接	简介。
线程池?	简介。