GoogleCartographerROS源码解析

Google Cartographer ROS overview

Cartographer主要可分为两个子系统&＃xff1a;local SLAM(frontend or local trajectory builder), 和global SLAM (backend)。

Local SLAM构建一系列的submaps&＃xff0c;每一个submaps具有局部一致性&＃xff0c;可理解为其submaps内部是进行了匹配&＃xff0c;但是各个submaps之间随着时间会产生漂移。

Global SLAM background运行&＃xff0c;利用scan-matching scans找到loop closure constraints&＃xff0c;然后利用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

优点&＃xff1a;算法低功耗、实时、不追求高精度(5cm)&＃xff1b;算法实现完美值得学习、易于扩展、维护&＃xff1b;开发依赖库很少&＃xff0c;主要包括Boost&＃xff0c;Eigen3&＃xff0c;Lua&＃xff0c;Ceres&＃xff0c;Protobuf&＃xff0c;产品级的嵌入式机器人系统。

应用场景&＃xff1a;计算资源有限、对精度要求不高、且需要实时避障的、规划、导航的应用&＃xff0c;如室内服务机器人、无人机、以及物流、餐饮、安防、医疗等等。

Cartographer ROS Code Explanation

把代码主要分为两个部分&＃xff0c;即cartographer_ros (ROS接口)和cartographer (Local SLAM和Global SLAM).

本文旨在以最简洁的方式理清代码运行的思路。

cartographer_ros (ROS接口)

cartographer_ros这个package是在ROS下面运行的&＃xff0c;可以以ROS消息的方式接受各种传感器数据&＃xff0c;在处理过后又以消息的形式publish出去&＃xff0c;便于调试和可视化。

main():
整个程序的入口main函数在cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc文件中&＃xff0c;通过void Run()来启动整个程序&＃xff0c;包括&＃xff1a;

• 加载参数&＃xff1a;NodeOptions&＃xff0c;TrajectoryOptions&＃xff1b;
• 定义了map_builder变量&＃xff1a;请注意此时的变量类型已经是cartographer::mapping::MapBuilder了&＃xff1b;
• Node类&＃xff1a;所有的一切都送入了Node这个类。

class Node:
而在Node类的构造函数中&＃xff0c;包括&＃xff1a;

• 发布topic

  • kSubmapListTopic
  • kTrajectoryNodeListTopic
  • kLandmarkPosesListTopic
  • kConstraintListTopic
  • kScanMatchedPointCloudTopic (匹配上的点云数据)

• 服务service

  • kSubmapQueryServiceName(查询Submap)
  • kStartTrajectoryServiceName(开始一段trajectory)
  • kFinishTrajectoryServiceName(结束一段trajectory)
  • kWriteStateServiceName

开始运行构建地图&＃xff0c;都需要调用一个重要的函数int Node::AddTrajectory(const TrajectoryOptions& options,const cartographer_ros_msgs::SensorTopics& topics)&＃xff0c;而这个函数可以通过两种方式调用&＃xff0c;一是通过调用service函数&＃xff0c;启动bool Node::HandleStartTrajectory();另一个是在void Run()中使用默认topics直接调用void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options)。

函数int Node::AddTrajectory()开始一段trajectory&＃xff0c;添加了

• PoseExtrapolator 估计pose
• SensorSamplers 处理传感器
• LaunchSubscribers 消息订阅

另外&＃xff0c;map_builder_bridge_变量也同时调用了AddTrajectory()函数&＃xff0c;而其是在Node构建函数参数列表初始化map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer),由main()传入。

订阅传感器发布的消息&＃xff1a;Laser, MultiEchoLaser, PointCloud2, IMU, Odometry, NavSatFixMessage, Landmark等&＃xff0c;并调用了相应的处理函数&＃xff0c;最后都以map_builder_bridge_.sensor_bridge->HandleLaserScanMessage的形式进行了调用&＃xff0c;此处需要注意的是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定义&＃xff0c;这使得接下来的功能转到MapBuilderInterface。

class MapBuilderBridge中定义了对象sensor_bridges_来使用class SensorBridge&＃xff0c;在处理各种传感信息时&＃xff0c;都调用了trajectory_builder_->AddSensorData()&＃xff0c;通过不同的sensor_id和变量类型添加不同的消息&＃xff0c;Laser, MultiEchoLaser, PointCloud2, IMU, Odometry, NavSatFixMessage, Landmark等&＃xff0c;而这些由cartographer::mapping::TrajectoryBuilderInterface类接口连接。

ros部分脉络&＃xff1a;cartographer_ros这部分帮助更加方便的接受各种消息&＃xff0c;方便开发调试。系统初始化流程&＃xff1a;从node_main.cc文件中void Run()->class Node构造函数定义发布topics和service->Node::AddTrajectory()订阅各种传感器消息->class MapBuilderBridge和class SensorBridge实现与cartographer的桥接->调用cartographer::mapping::MapBuilderInterface和cartographer::mapping::TrajectoryBuilderInterface接口类。

cartographer

cartographer源码主要包括文件夹&＃xff1a;cloud、common、ground_truth、io、mapping、metrics、sensor、transform&＃xff0c;

• mapping: 算法的核心部分&＃xff0c;包括ROS调用部分的接口、submap的构建、位姿优化接口&＃xff1b;
• common&＃xff1a;基本数据结构、工具接口&＃xff1b;
• sensor: 传感器数据结构&＃xff1b;
• transform: 位姿数据结构、位姿变换&＃xff1b;
• io: 存读取数据、日志&＃xff1b;
• cloud: 点云处理&＃xff1b;

configuration_files文件定义了map_builder,pose_graph,trajectory_builder的配信信息&＃xff0c;Lua是一种非常轻量的脚本语言&＃xff0c;主要用来做Configuration。

接口class MapBuilderInterface&＃xff1a;接口MapBuilderInterface具体由MapBuilder继承并实现&＃xff0c;MapBuilder是cartographer算法的顶层类&＃xff0c;包括两个部分&＃xff1a;TrajectoryBuilder类建立与维护Local Submap&＃xff08;Local SLAM&＃xff09;&＃xff0c;PoseGraph全局pose Loop Closure&＃xff08;Global SLAM&＃xff09;。

class MapBuilder&＃xff1a;在/mapping/map_builder.h中定义了一个PoseGraph的智能指针std::unique_ptr pose_graph_,一个收集传感器数据的指针std::unique_ptr sensor_collator_,所有trajectory的管理向量std::vector> trajectory_builders_及其相应的Configuratonstd::vector all_trajectory_builder_options_。

MapBuilder中TrajectoryBuilder根据trajectory上收集到的传感器数据构建出栅格地图submap&＃xff0c;submap会随着时间进行累积&＃xff0c;超过阈值就会新增一个submap&＃xff1b;PoseGraph则根据loop closure的constrants将所有的submaps进行全局优化&＃xff0c;构建Global SLAM。

Local SLAM

TrajectoryBuilder自然是用来创建trajectory的&＃xff0c;通过抽取trajectory上采集的数据获得关键帧并对应到trajectory的node上&＃xff0c;trajectory就是一串node&＃xff1b;同时需要建立栅格地图submaps&＃xff0c;以便进行做scanmatch&＃xff0c;形成global map。

在/mapping/trajectory_builder_interface.h中struct InsertionResult{}用于保存Local SLAM的一个节点的数据结构&＃xff0c;包括NodeId&＃xff0c;TrajectoryNode::Data&＃xff0c;Submap。
NodeId在/mapping/id.h中定义&＃xff0c;由两部分组成&＃xff1a;一个int型trajectory_id和一个int型node_index。而TrajectoryNode包括时间、传感器数据、节点在Local SLAM中的Pose、点云、节点在世界坐标系下的位姿。
在/mapping/submaps.h中Submap不停的将range data加入其中&＃xff0c;当达到一定阈值&＃xff0c;submap完成&＃xff0c;就要寻找constrains执行loop closure。

LocalSlamResultCallback函数是在local SLAM处理accumulated Rangedata 之后调用&＃xff0c;插入到Local Slam中的pose&＃xff0c;RangeData数据和插入结果InsertionResult。

virtual void AddSensorData()处理传感器数据的5个纯虚函数。

TrajectoryBuilderInterface会在其继承类CollatedTrajectoryBuilder (/mapping/internal/collated_trajectory_builder.h)继承并实现具体方法。

在class MapBuilder中&＃xff0c;有一个对象trajectory_builders_(接口std::vector> trajectory_builders_),在int MapBuilder::AddTrajectoryBuilder()
函数中进行了赋值&＃xff0c;指针类行为CollatedTrajectoryBuilder,在类CollatedTrajectoryBuilder的构造函数中&＃xff0c;利用GlobalTrajectoryBuilder中的函数CreateGlobalTrajectoryBuilder2D()提供输入。在同样LocalTrajectoryBuilder2D和PoseGraph2D也为CreateGlobalTrajectoryBuilder2D()提供输入&＃xff0c;用于创建GlobalTrajectoryBuilder,最后使得三个类绑在一起&＃xff0c;具体的实现例子如下&＃xff08;必须贴出来&＃xff0c;以免混乱&＃xff09;&＃xff1a;

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的相关类之间的调用关系应该是&＃xff1a;MapBuilderInterface->MapBuilder->TrajectoryBuilderInterface->CollatedTrajectoryBuilder->GlobalTrajectoryBuilder->CreateGlobalTrajectoryBuilder2D->LocalTrajectoryBuilder2D。

MapBuilder初始化&＃xff1a;
cartographer_ros利用MapBuilderBridge调用MapBuilderInterface->MapBuilder->int Mapuilder::AddTrajectoryBuilder(),而在继承实现TrajectoryBuilderInterface的类CollatedTrajectoryBuilder的构造函数输入参数中&＃xff0c;将类GlobalTrajectoryBuilder和LocalTrajectoryBuilder2D传入&＃xff0c;取名为wrapped_trajectory_builder(取名很讲究)。

传感器Topic消息接收&＃xff1a;在ROS订阅topic的函数中&＃xff0c;利用SensorBridge来实现桥接&＃xff0c;以laser为例&＃xff0c;SensorBridge::HandleLaserScanMessage&＃xff0c;最终还是需要调用trajectory_builder_->AddSensorData()&＃xff0c;由CollatedTrajectoryBuilder、GlobalTrajectoryBuilder继承TrajectoryBuilder实现AddSensorData()函数&＃xff0c;最后在LocalTrajectoryBuilder2D类中实现PoseExtrapolator和Scan Matching等核心算法。

在接收到IMU、odometry消息后由PoseExtrapolator压入队列作为插入激光雷达的辅助&＃xff0c;而当LocalTrajectoryBuilder2D::AddRangeData()时&＃xff0c;则结合起来处理核心激光点云的数据。

Global SLAM

当local SLAM生成一串连续的submaps&＃xff0c;全局优化在后台运行&＃xff0c;重新排列submaps&＃xff0c;使得形成全局一致的地图。pose graph优化通过建立nodes和submaps之间的constraints&＃xff0c;然后优化pose graph。

PoseGraph先由接口PoseGraphInterface((/mapping/pose_graph_interface.h中))定义&＃xff0c;然后由PoseGraph (/mapping/pose_graph.h)来继承&＃xff0c;再区分不同的2D和3D的情况由PoseGraph2D (/mapping/internal/2d/pose_graph_2d.h)和PoseGraph3D (/mapping/internal/3d/pose_graph_3d.h)实现。

PoseGraphInterface中定义了约束的数据结构Constraint&＃xff0c;LandmarkNode,Submap,TrajectoryData,以及全局优化的回调函数GlobalSlamOptimizationCallback。而在PoseGraph (/mapping/pose_graph.h)中又定义了大量的虚函数&＃xff0c;最后在PoseGraph2D (/mapping/internal/2d/pose_graph_2d.h)中实现。

enum class SubmapState { kActive, kFinished }描述了当前submap的状态&＃xff0c;而当状态转换到kFinished的时候&＃xff0c;所有的nodes都要跟submap做一次匹配&＃xff1b;同样的&＃xff0c;当trajetory中由新增的nodes时&＃xff0c;新的nodes也需要跟finished submaps全部做一次匹配。
利用AddNode()函数&＃xff0c;PoseGraph需要不断的将trajectory上新增的TrajectoryNode添加到其中&＃xff1b;PoseGraph检查insertion_submaps中Old Submap状态是否为finished&＃xff0c;如是则需要进行一次Loop Closure。

AddNode()调用了一个重要的函数PoseGraph2D::ComputeConstraintsForNode(),用于计算新增节点与所有submaps的相对位姿约束。
void ComputeConstraint()计算新的节点与旧的submaps之间的关系&＃xff0c;而void ComputeConstraintsForOldNodes()是当一个新的submap finished的时候&＃xff0c;计算新submaps与旧节点之间的关系。

Non-global constraints在traject上节点之间的约束(即内部submaps的约束)&＃xff0c;保持了trajectory局部结构的一致性。Global constraints(即loop closure constraints)由新的submap和之前旧的nodes进行搜索匹配得到。首先采用FastCorrelativeScanMatcher&＃xff0c;能够实时的loop closures scan matching&＃xff0c;采用“Branch and bound” 的机制&＃xff0c;高效消除不正确的匹配。当找到合适的备选项之后&＃xff0c;在采用Ceres Scan Matcher来优化pose。

optimization旨在重新调整submaps&＃xff0c;根据多个不同权重的残差&＃xff0c;包括&＃xff1a;global (loop closure) constraints&＃xff0c;non-global (matcher) constraints&＃xff0c;IMU acceleration及rotation measurements&＃xff0c;local SLAM rough pose estimations&＃xff0c;an odometry source&＃xff0c;a fixed frame (such as a GPS system)。具体的处理在2D::HandleWorkQueue()(/mapping/internal/2d/pose_graph_2d.h))中&＃xff0c;根据ConstraintBuilder2D输入的约束关系。其由PoseGraph2D中constraint_builder_(constraints::ConstraintBuilder2D)在加入新的Node时&＃xff0c;建立新Node与submap之间的约束关系&＃xff0c;存于vector constraints_&＃xff0c;最后调用RunOptimization()函数进行全局优化。

Reference

1. 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.)

2. 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.)

3. 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.)

4. Cartographer ROS Integration
   

5. cartographer源码详细解读-April Lee