Google Cartographer ROS源码解析
Google Cartographer ROS overview
Cartographer主要可分为两个子系统：local SLAM(frontend or local trajectory builder), 和global SLAM (backend)。
Local SLAM构建一系列的submaps，每一个submaps具有局部一致性，可理解为其submaps内部是进行了匹配，但是各个submaps之间随着时间会产生漂移。
Global SLAM background运行，利用scan-matching scans找到loop closure constraints，然后利用pose graph优化找到全局一致的地图。
W. Hess, D. Kohler, H. Rapp, and D. Andor, Real-Time Loop Closure in 2D LIDAR SLAM, in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016. pp. 1271–1278
优点：算法低功耗、实时、不追求高精度(5cm)；算法实现完美值得学习、易于扩展、维护；开发依赖库很少，主要包括Boost，Eigen3，Lua，Ceres，Protobuf，产品级的嵌入式机器人系统。
应用场景：计算资源有限、对精度要求不高、且需要实时避障的、规划、导航的应用，如室内服务机器人、无人机、以及物流、餐饮、安防、医疗等等。
Cartographer ROS Code Explanation
把代码主要分为两个部分，即cartographer_ros (ROS接口)和cartographer (Local SLAM和Global SLAM).
本文旨在以最简洁的方式理清代码运行的思路。
cartographer_ros (ROS接口)
cartographer_ros这个package是在ROS下面运行的，可以以ROS消息的方式接受各种传感器数据，在处理过后又以消息的形式publish出去，便于调试和可视化。
main():
整个程序的入口main函数在cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc文件中，通过void Run()来启动整个程序，包括：
- 加载参数：NodeOptions，TrajectoryOptions；
- 定义了map_builder变量：请注意此时的变量类型已经是cartographer::mapping::MapBuilder了；
- Node类：所有的一切都送入了Node这个类。
class Node:
而在Node类的构造函数中，包括：
-
发布topic
- kSubmapListTopic
- kTrajectoryNodeListTopic
- kLandmarkPosesListTopic
- kConstraintListTopic
- kScanMatchedPointCloudTopic (匹配上的点云数据)
-
服务service
- kSubmapQueryServiceName(查询Submap)
- kStartTrajectoryServiceName(开始一段trajectory)
- kFinishTrajectoryServiceName(结束一段trajectory)
- kWriteStateServiceName
开始运行构建地图，都需要调用一个重要的函数int Node::AddTrajectory(const TrajectoryOptions& options,const cartographer_ros_msgs::SensorTopics& topics)，而这个函数可以通过两种方式调用，一是通过调用service函数，启动bool Node::HandleStartTrajectory();另一个是在void Run()中使用默认topics直接调用void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options)。
函数int Node::AddTrajectory()开始一段trajectory，添加了
- PoseExtrapolator 估计pose
- SensorSamplers 处理传感器
- LaunchSubscribers 消息订阅
另外，map_builder_bridge_变量也同时调用了AddTrajectory()函数，而其是在Node构建函数参数列表初始化map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer),由main()传入。
订阅传感器发布的消息：Laser, MultiEchoLaser, PointCloud2, IMU, Odometry, NavSatFixMessage, Landmark等，并调用了相应的处理函数，最后都以map_builder_bridge_.sensor_bridge->HandleLaserScanMessage的形式进行了调用，此处需要注意的是map_builder_bridge_和sensor_bridge都是桥接cartographer的关键。
class MapBuilderBridge:
class Node类主要是通过class MapBuilderBridge来实现再次调用
- Node::HandleSubmapQuery(){map_builder_bridge_.HandleSubmapQuery(request, response);}
- Node::PublishSubmapList(){map_builder_bridge_.GetSubmapList();}
- Node::PublishTrajectoryStates(){map_builder_bridge_.GetTrajectoryStates();}
- Node::PublishTrajectoryNodeList(){map_builder_bridge_.GetTrajectoryNodeList();}
- Node::PublishLandmarkPosesList)(){map_builder_bridge_.GetLandmarkPosesList();}
- Node::PublishConstraintList(){map_builder_bridge_.GetConstraintList();}
class MapBuilderBridge中对象map_builder_由类cartographer::mapping::MapBuilderInterface定义，这使得接下来的功能转到MapBuilderInterface。
class MapBuilderBridge中定义了对象sensor_bridges_来使用class SensorBridge，在处理各种传感信息时，都调用了trajectory_builder_->AddSensorData()，通过不同的sensor_id和变量类型添加不同的消息，Laser, MultiEchoLaser, PointCloud2, IMU, Odometry, NavSatFixMessage, Landmark等，而这些由cartographer::mapping::TrajectoryBuilderInterface类接口连接。
ros部分脉络：cartographer_ros这部分帮助更加方便的接受各种消息，方便开发调试。系统初始化流程：从node_main.cc文件中void Run()->class Node构造函数定义发布topics和service->Node::AddTrajectory()订阅各种传感器消息->class MapBuilderBridge和class SensorBridge实现与cartographer的桥接->调用cartographer::mapping::MapBuilderInterface和cartographer::mapping::TrajectoryBuilderInterface接口类。
cartographer
cartographer源码主要包括文件夹：cloud、common、ground_truth、io、mapping、metrics、sensor、transform，
- mapping: 算法的核心部分，包括ROS调用部分的接口、submap的构建、位姿优化接口；
- common：基本数据结构、工具接口；
- sensor: 传感器数据结构；
- transform: 位姿数据结构、位姿变换；
- io: 存读取数据、日志；
- cloud: 点云处理；
configuration_files文件定义了map_builder,pose_graph,trajectory_builder的配信信息，Lua是一种非常轻量的脚本语言，主要用来做Configuration。
接口class MapBuilderInterface：接口MapBuilderInterface具体由MapBuilder继承并实现，MapBuilder是cartographer算法的顶层类，包括两个部分：TrajectoryBuilder类建立与维护Local Submap（Local SLAM），PoseGraph全局pose Loop Closure（Global SLAM）。
class MapBuilder：在/mapping/map_builder.h中定义了一个PoseGraph的智能指针std::unique_ptr,一个收集传感器数据的指针std::unique_ptr,所有trajectory的管理向量std::vector及其相应的Configuratonstd::vector。
MapBuilder中TrajectoryBuilder根据trajectory上收集到的传感器数据构建出栅格地图submap，submap会随着时间进行累积，超过阈值就会新增一个submap；PoseGraph则根据loop closure的constrants将所有的submaps进行全局优化，构建Global SLAM。
Local SLAM
TrajectoryBuilder自然是用来创建trajectory的，通过抽取trajectory上采集的数据获得关键帧并对应到trajectory的node上，trajectory就是一串node；同时需要建立栅格地图submaps，以便进行做scanmatch，形成global map。
在/mapping/trajectory_builder_interface.h中struct InsertionResult{}用于保存Local SLAM的一个节点的数据结构，包括NodeId，TrajectoryNode::Data，Submap。
NodeId在/mapping/id.h中定义，由两部分组成：一个int型trajectory_id和一个int型node_index。而TrajectoryNode包括时间、传感器数据、节点在Local SLAM中的Pose、点云、节点在世界坐标系下的位姿。
在/mapping/submaps.h中Submap不停的将range data加入其中，当达到一定阈值，submap完成，就要寻找constrains执行loop closure。
LocalSlamResultCallback函数是在local SLAM处理accumulated Rangedata 之后调用，插入到Local Slam中的pose，RangeData数据和插入结果InsertionResult。
virtual void AddSensorData()处理传感器数据的5个纯虚函数。
TrajectoryBuilderInterface会在其继承类CollatedTrajectoryBuilder (/mapping/internal/collated_trajectory_builder.h)继承并实现具体方法。
在class MapBuilder中，有一个对象trajectory_builders_(接口std::vector),在int MapBuilder::AddTrajectoryBuilder()
函数中进行了赋值，指针类行为CollatedTrajectoryBuilder,在类CollatedTrajectoryBuilder的构造函数中，利用GlobalTrajectoryBuilder中的函数CreateGlobalTrajectoryBuilder2D()提供输入。在同样LocalTrajectoryBuilder2D和PoseGraph2D也为CreateGlobalTrajectoryBuilder2D()提供输入，用于创建GlobalTrajectoryBuilder,最后使得三个类绑在一起，具体的实现例子如下（必须贴出来，以免混乱）：
int MapBuilder::AddTrajectoryBuilder(){}
std::unique_ptr local_trajectory_builder;
...
trajectory_builders_.push_back(
common::make_unique(
sensor_collator_.get(), trajectory_id, expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast(pose_graph_.get()),
local_slam_result_callback)));
所以MapBuilder的相关类之间的调用关系应该是：MapBuilderInterface->MapBuilder->TrajectoryBuilderInterface->CollatedTrajectoryBuilder->GlobalTrajectoryBuilder->CreatGlobalTrajectoryBuilder2D->LocalTrajectoryBuilder2D。
MapBuilder初始化：
cartographer_ros利用MapBuilderBridge调用MapBuilderInterface->MapBuilder->int Mapuilder::AddTrajectoryBuilder(),而在继承实现TrajectoryBuilderInterface的类CollatedTrajectoryBuilder的构造函数输入参数中，将类GlobalTrajectoryBuilder和LocalTrajectoryBuilder2D传入，取名为wrapped_trajectory_builder(取名很讲究)。
传感器Topic消息接收：在ROS订阅topic的函数中，利用SensorBridge来实现桥接，以laser为例，SensorBridge::HandleLaserScanMessage，最终还是需要调用trajectory_builder_->AddSensorData()，由CollatedTrajectoryBuilder、GlobalTrajectoryBuilder继承TrajectoryBuilder实现AddSensorData()函数，最后在LocalTrajectoryBuilder2D类中实现PoseExtrapolator和Scan Matching等核心算法。
在接收到IMU、odometry消息后由PoseExtrapolator压入队列作为插入激光雷达的辅助，而当LocalTrajectoryBuilder2D::AddRangeData()时，则结合起来处理核心激光点云的数据。
Global SLAM
当local SLAM生成一串连续的submaps，全局优化在后台运行，重新排列submaps，使得形成全局一致的地图。pose graph优化通过建立nodes和submaps之间的constraints，然后优化pose graph。
PoseGraph先由接口PoseGraphInterface((/mapping/pose_graph_interface.h中))定义，然后由PoseGraph (/mapping/pose_graph.h)来继承，再区分不同的2D和3D的情况由PoseGraph2D (/mapping/internal/2d/pose_graph_2d.h)和PoseGraph3D (/mapping/internal/3d/pose_graph_3d.h)实现。
PoseGraphInterface中定义了约束的数据结构Constraint，LandmarkNode,Submap,TrajectoryData,以及全局优化的回调函数GlobalSlamOptimizationCallback。而在PoseGraph (/mapping/pose_graph.h)中又定义了大量的虚函数，最后在PoseGraph2D (/mapping/internal/2d/pose_graph_2d.h)中实现。
enum class SubmapState { kActive, kFinished }描述了当前submap的状态，而当状态转换到kFinished的时候，所有的nodes都要跟submap做一次匹配；同样的，当trajetory中由新增的nodes时，新的nodes也需要跟finished submaps全部做一次匹配。
利用AddNode()函数，PoseGraph需要不断的将trajectory上新增的TrajectoryNode添加到其中；PoseGraph检查insertion_submaps中Old Submap状态是否为finished，如是则需要进行一次Loop Closure。
AddNode()调用了一个重要的函数PoseGraph2D::ComputeConstraintsForNode(),用于计算新增节点与所有submaps的相对位姿约束。
void ComputeConstraint()计算新的节点与旧的submaps之间的关系，而void ComputeConstraintsForOldNodes()是当一个新的submap finished的时候，计算新submaps与旧节点之间的关系。
Non-global constraints在traject上节点之间的约束(即内部submaps的约束)，保持了trajectory局部结构的一致性。Global constraints(即loop closure constraints)由新的submap和之前旧的nodes进行搜索匹配得到。首先采用FastCorrelativeScanMatcher，能够实时的loop closures scan matching，采用“Branch and bound” 的机制，高效消除不正确的匹配。当找到合适的备选项之后，在采用Ceres Scan Matcher来优化pose。
optimization旨在重新调整submaps，根据多个不同权重的残差，包括：global (loop closure) constraints，non-global (matcher) constraints，IMU acceleration及rotation measurements，local SLAM rough pose estimations，an odometry source，a fixed frame (such as a GPS system)。具体的处理在2D::HandleWorkQueue()(/mapping/internal/2d/pose_graph_2d.h))中，根据ConstraintBuilder2D输入的约束关系。其由PoseGraph2D中constraint_builder_(constraints::ConstraintBuilder2D)在加入新的Node时，建立新Node与submap之间的约束关系，存于vector，最后调用RunOptimization()函数进行全局优化。
Reference
-
Cartographer (Hess, W., Kohler, D., Rapp, H., & Andor, D. (2016, May). Real-time loop closure in 2D LIDAR SLAM. In 2016 IEEE International Conference on Robotics and Automation (ICRA) (pp. 1271-1278). IEEE.)
-
Sparse Pose Adjustment (Konolige, K., Grisetti, G., Kümmerle, R., Burgard, W., Limketkai, B., & Vincent, R. (2010, October). Efficient sparse pose adjustment for 2D mapping. In 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (pp. 22-29). IEEE.)
-
Branch-and-bound scan matching (Olson, E. B. (2009, May). Real-time correlative scan matching. In 2009 IEEE International Conference on Robotics and Automation (pp. 4387-4393). IEEE.)
-
Cartographer ROS Integration
-
cartographer源码详细解读-April Lee