1 介绍1.1 概述Cartographer是一款可以跨多个平台和传感器配置提供2D和3D实时同步定位和绘图(SLAM)的系统。这个项目提供了Cartographer的ROS集成。Cartographer是google推出的一套基于图优化的SLAM算法。Cartographer算法并没有给人惊艳的感觉，但该算法的主要目标是实现低计算资源消耗，达到实时SLAM的目的。该算法主要分为两个部分，第一个部分称为Local SLAM, 该部分通过一帧帧的Laser Scan建立并维护一系列的Submap，而所谓的submap就是一系列的Grid Map。当再有新的Laser Scan中会通过Ceres Scan Matching的方法将其插入到子图中的最佳位置。但是submap会产生误差累积的问题，因此，算法的第二个部分，称为Global SLAM的部分，就是通过Loop Closure来进行闭环检
1 介绍
1.1 概述
Cartographer是一款可以跨多个平台和传感器配置提供2D和3D实时同步定位和绘图(SLAM)的系统。这个项目提供了Cartographer的ROS集成。Cartographer是google推出的一套基于图优化的SLAM算法。Cartographer算法并没有给人惊艳的感觉，但该算法的主要目标是实现低计算资源消耗，达到实时SLAM的目的。该算法主要分为两个部分，第一个部分称为Local SLAM, 该部分通过一帧帧的Laser Scan建立并维护一系列的Submap，而所谓的submap就是一系列的Grid Map。当再有新的Laser Scan中会通过Ceres Scan Matching的方法将其插入到子图中的最佳位置。但是submap会产生误差累积的问题，因此，算法的第二个部分，称为Global SLAM的部分，就是通过Loop Closure来进行闭环检测，来消除累积误差：当一个submap构建完成，也就是不会再有新的laser scan插入到该submap时，算法会将该submap加入到闭环检测中。闭环检测的本质也是一个优化问题，该优化问题被表达成了一个pixel-accurate match的形式，解决优化问题的方法是Branch-and-Bound Approach.Cartographer本身是一个C++的库，虽然可以不依赖ROS的环境运行，但为了快速的上手，google还是提供了一个ROS环境下的封装cartographer_ros。
1.2 评价
我认为Cartographer这个库最重要的东西还不是算法，而是实现。 2D/3D的SLAM的核心部分仅仅依赖于以下几个库：
Boost：准标准的C++库。
Eigen3： 准标准的线性代数库。
Lua：非常轻量的脚本语言，主要用来做Configuration
Ceres：这是Google开源的做非线性优化的库，仅依赖于Lapack和Blas
Protobuf：这是Google开源的很流行的跨平台通信库
没有PCL，g2o, iSAM, sophus, OpenCV, ROS 等等，所有轮子都是自己造的。这明显不是搞科研的玩儿法，就是奔着产品去的。前面说过，算法需要的计算资源少，而且因为依赖很少，因此几乎可以直接应用在一个产品级的嵌入式系统上。以前学术界出来的开源2D/3D SLAM算法不少，但能几乎直接拿来就用在产品上的，恕我孤陋寡闻还真想不出来。因此，我认为进入相关领域SLAM算法的门槛被显著降低了。
1.3 特点
工程化代码，依赖少，算法设计精妙，能在嵌入式主控上运行。适合产品级应用；
支持 ROS
优秀的软件架构设计，便于接入IMU、（单/多线）雷达、里程计、甚至为二维码辅助等视觉识别方式也预留了接口（Landmark）。
Cartographer支持2D和3D激光雷达的输入，实现机器人定位，并构建栅格地图。
2D-SLAM：基于2D栅格地图，可以直接用于导航。
3D-SLAM：基于hybridGrid，译为混合概率地图。
2 框架
官方
3 代码结构
heimazaifei 解读
mapBuilder：实现整个地图构建，包括前端local slam和后端 global slam。
轨迹（trajectory): 可以理解为一次SLAM 从起点到终点过程中的机器人行走轨迹，建图中可以通过startTrajectory和finishTrajectory控制。在轨迹生成的过程中，完成sensor到sumap的生成，以及pose_graph的构建。TrajectoryBuilder(globalTrajectory类)主要通过sensor_collator(localTrajecory类)和pose_graph构成 。sensor_collator实现局部地图构建，最终结果传递给pose_graph；
节点图(poseGraph)：具体参考图优化的知识。简单理解图优化（如果没接触过图优化，下面估计看不懂，后续会详细讲）：每个插入的激光和生成sumap，以及landmark（后续讲）都可以理解为图优化的一个节点，建图过程中，生成这些点之间的关系，这些关系便是图中的线，最终优化，就是调整点的位置，得到最优值。可以理解为PoseGraph主要实现全局优化（global slam）功能。
代码流程：最终代码运行通过ROS node 方式实现。node中对应topic和service订阅和发布等功能通过MapBuilderBridge类实现。MapBuilderBridge顾名思义，实现ROS节点代码和cartographer功能代码之间的桥接，也可以理解为对cartographer主体代码的接口封装。cartographer主体代码主要功能通过构建地图MapBuilder类实现。此外，MapBuilderBridge 还包含sensorBridge类，实现传感信息ros格式和cartographer自定义格式之间的转换，这些传感器主要包括scan,imu,odom,tf 等。
linyicheng 解读
https://github.com/linyicheng1/OpenSLAM-Notes/tree/main/cartographer-master
Xiaotu 解读
https://gaoyichao.com/Xiaotu/?book=Cartographer%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB&title=index
cartographer_ros
地图构建器map_builder
实际完成地图构建的就是在系统运行之初所构建的map_builder对象。 它是Catographer的顶层对象， 为用户提供了一套统一的接口MapBuilderInterface。 封装了对象pose_graph_和trajectory_builders_来分别完成闭环检测和子图构建。还构建了一个具有固定数量的线程池，用于管理多线程。
| 重点 | 说明 |
|---|---|
| 初识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优化是一个很耗时的过程，期间前端可能会产生新的子图和路径节点，所以在完成一次优化之后应当调整新增的路径节点的位姿估计。
| 重点 | 说明 |
|---|---|
| Global SLAM的核心——PoseGraph2D | 在Cartographer中的位姿图是由轨迹节点和子图构成的二部图，图中的约束描述的是轨迹节点与子图之间的位置关系。后端优化就是估计轨迹节点与子图在世界坐标系下的位姿， 最小化全局估计与局部估计之间的偏差。本文我们将研究一下PoseGraph2D的成员变量，以及与位姿图相关的一些数据结构。 |
| 位姿图的创建与更新 | 本文我们将详细分析位姿图PoseGraph2D的构造与更新过程。将看到PoseGraph2D通过对象constraint_builder_在后台完成闭环检测构建约束。 它还有一个工作队列来协调后台的任务。 |
| 约束构建器——constraint_builder_ | 本文中我们将详细分析约束构建器constraint_builder_，将看到它是如何通过MaybeAdd-WhenDone调用循环，借助线程池来组织闭环检测和约束的并行计算。 |
| 分支定界闭环检测的原理和实现 | Cartographer使用类FastCorrelativeScanMatcher2D具体实现了深度优先的分支定界搜索算法，该算法能够高效地进行扫描匹配，计算路径节点与子图之间的约束关系。 |
| 后端优化过程 | 本文我们将回到Global SLAM的系统框图，来了解经过闭环检测构建了子图与路径节点之间的约束之后，都做了些什么工作。 通过简要的分析后端优化求解器的数据结构以及基于Ceres库进行SPA(Sparse Pose Adjustment)的优化方法，来了解后端优化的过程。 |
赵锴 解读
SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
地图设计
Cartographer的地图（map）以子地图（submap）的形式组成。
分为前端和后端。 前端：根据帧间匹配算法（scan-match），实时根据激光(scan)来推测累积的scan相对于submap的位姿。 后端：检测回环（发现在已到达的位置附近），修正各个submap之间的位姿。
根据代码可以判断，2D和3D基于的是同一套思路，但是在实现上有一定区别。 接下来结合2D和3D部分，对比介绍实现定位和建图的方法。
在介绍定位和建图思路之前，先介绍一下地图的更新方式：
发出一束激光，打到一个障碍物点，被打到的称为hit点，中间连线上的空区域，称为miss点。
2d和3d都是存储的这样的地图。3d相当于是3维的栅格地图。
宏观上：多次观测到，提升其概率。
\frac{p}{1-p} odds(p)\=1−pp M n e w ( x ) = c l a m p ( o d d s − 1 ( o d d s ( m o l d ( x ) ) . o d d s ( p h i t ) ) ) M_{new}(x)=clamp(odds^{-1}(odds(m_{old}(x)).odds(p_{hit}))) Mnew(x)\=clamp(odds−1(odds(mold(x)).odds(phit)))
p表示占据概率，当p=0.5时，概率比值odds=1，表示占据和空闲各占一半。 o d d s − 1 odds^{-1} odds−1表示函数逆运算。
p h i t = 0.55 p_{hit}=0.55 phit\=0.55代表该位置被激光打到一次的概率，第一次观测会被直接赋值。
M n e w ( x ) M_
初始时刻，栅格未知状态，激光第一次打到了位置 x 处，M(x)概率更新为0.55。
随后，激光第二次重复打到了同一个位置： o d d s ( p h i t ) = 0.55 1 − 0.55 = 1.22 odds(p_{hit})=\frac{0.55} {1-0.55}=1.22 odds(phit)\=1−0.550.55\=1.22, o d d s ( M o l d ( x ) ) = o d d s 0.55 = 1.22 odds(M_{old}(x))=odds_{0.55}=1.22 odds(Mold(x))\=odds0.55\=1.22 o d d s ( p h i t ) odds(p_{hit}) odds(phit) 和 o d d s ( M o l d ( x ) ) odds(M_{old}(x)) odds(Mold(x)) 相乘为1.484，再求函数逆运算，恢复出更新后的 M n e w ( x ) = 0.597 M_{new}(x)=0.597 Mnew(x)\=0.597。
实际代码中，采用了多种工程技巧加速运算。包括：映射到整数范围，预计算，查表等方法，此处不深入展开了。
匹配方法
scan-scan： 这个意味着利用两帧激光数据（每帧激光束的数目相同），计算二者之间的变换。典型方法：ICP。 scan-map： 利用一帧激光数据和地图数据，找到激光数据在地图中的位置。 map-map： 利用一个子地图数据，在一个更大的地图中找到它合适的位置。
2D和3D的前端，Cartographer采用的是scan-map的匹配方法。
不管是2D还是3D，首先要有一个初始的位姿，在此基础上进行优化：
有IMU，则采纳其角速度积分作为初始姿态。不信任IMU任何加速度信息。
有里程计，则采纳里程计的线速度积分作为初始平移。
二者都没有，根据之前的运动做一个匀速的假设。
在得到了初始位姿以后，初始位姿要经过第一阶段解算：CSM（Correlation Scan Match 相关扫描匹配）——构建似然场。
即对原先的地图map进行一个高斯模糊，让它膨胀一些，然后把激光scan在一个搜索窗口内暴力匹配，计算得分。
注意，这里有两个问题： 1.得分怎么算？ 如果scan的点落在障碍物模糊区域内，落的越多，得分越高。 2.地图不是无限大的吗，你怎么保证在搜索窗口里就能找到位姿呢？ 因为有初始位姿。误差肯定在一个范围内而不会马上发散到很远，所以可以在一个位姿的窗口内，对位姿进行暴力匹配搜索。（初始位姿估计中，里程计数据不会突然激增；imu的加速度信息会漂移，但是算法对于imu加速度数据选择直接丢弃不看；而根据之前位姿匀速假设也不会飘走）
这时候我们就要考虑：
什么是位姿？位置+姿态。
对于2D SLAM而言，有三个变量，x，y，yaw角。 对于3D SLAM而言，有x，y，z，roll，pitch，yaw六个变量。
2dslam中，采用三层循环，（最外层为θ，减小sin和cos的频繁计算），对x，y，θ在给定大小的搜索窗口内进行穷举，计算最高得分的x，y，θ作为一阶段解算的输出位姿。
3dslam中，采用六层循环，对x，y，z，roll，pitch，yaw六个变量在搜索窗口内穷举，计算得分最高的作为一阶段解算输出位姿。
很显然，3d-slam的这种方式对于计算资源依赖较大，复杂度达到O(n^6)级别。因此3d-slam的CSM方法，作为一个配置选项，默认是不开启的。当然如果用户机器比较牛逼，也可以选择开启。
二阶段解算
我们可以看出，第一阶段CSM解算中，位姿在其中是一个离散的变量，通过暴力枚举获得输出结果；
但是暴力枚举也是存在分辨率的，例如：如果角度步长设为1度，但如果刚好真正的角度是5.5度，那么CSM只能搜索到5或6度，而无法进一步细化，逐步累积将会造成误差。 因此，引入第二阶段位姿解算：非线性优化。
E ( T ) = a r g m i n T ∑ [ 1 − M ( S i ( T ) ) ] 2 E(T)=arg \mathop{min} \limits_{T}\sum[1-M(S_i(T))]^2 E(T)\=argTmin∑[1−M(Si(T))]2
S i ( T ) Si(T) Si(T) 表示把激光数据 S 用位姿 T 进行转换， M(x) 表示得到坐标 x 的地图占用概率。
思路：S代表了激光击中障碍物，将激光点在机器人坐标系下的位置，经过T转换到世界坐标系下以后，应该尽可能的落在已有地图的障碍物上。
第二阶段的位姿求解，显然位姿在其中是一个连续的变量，通过梯度下降的方法求解目标函数。 由于地图是离散的，因此需要对地图进行插值处理，使地图也变成一个可以求导的连续变量，这样才能优化前述目标函数。
[x0, x1] 区间内某一位置 x 在直线上的y值；
双线性插值本质上就是在两个方向上做线性插值。
阅读比较了代码，我判断2D和3D对于此部分内容基本相同。
2D：三个误差项：位姿转换误差+ 旋转误差+平移误差 ，后二者限制了旋转和平移的修改不能距离初始位姿太大。
3D：四个误差项：低分辨率位姿转换误差+ 高分辨率位姿转换误差+旋转误差+平移误差。低分辨率位姿转换误差权重低于高分辨率。
旋转和平移的权重也可以在配置文件中调参。
后端
Cartographer 在后端主要寻找回环，并根据建立的约束对所有的sumap进行统一优化。
回环检测目的是：检测当前位置是否曾经来过，即采用当前scan在历史中搜索，确认是否匹配。
为什么要有回环检测呢？原因有二：1. 已有地图时位姿初始化，不知道当前帧初始位姿，也就不清楚在地图中哪个位置，无法做定位。 2.有累积误差，仅靠前端递推，不进行修正的话，地图很容易变形。
因此接下来我们探讨两个问题：1.如何检测回环。2.检测回环后该怎么做。
如何检测回环
检测回环和前端的思路也比较相似，先通过穷举暴力匹配，再通过优化精细修正。
但是，前端的暴力穷举，是在有个初始位姿的基础上在一个小窗口内穷举。
后端重定位，没有初始位姿了，暴力匹配的范围变成了整个地图。
因此必须采用算法加速处理：多分辨率地图+分支定界操作。
假设有一帧激光：
发表评论