cartographer提供了实时并且可视化的建图，能够帮助操作员很好的评估建图质量和查看构建地图的范围，文章提供了一种5cm分辨率、实时建图和闭环的一种方法。

一. 介绍INTRODUCTION

本文的贡献：提供了一种新颖的计算闭环约束的方法，能够降低对大量激光数据的计算要求。该方法可以构建数万平方米的地图。

二. 相关工作

scan-to-scan

常用方法，但是容易积累误差。

scan-to-map

可以有效的减少累计误差。
其中一个代表性方法为利用高斯牛顿法查找最佳插值，详见下面的论文：

S. Kohlbrecher, J. Meyer, O. von Stryk, and U. Klingauf, “A flexible and scalable SLAM system with full 3D motion estimation,” in Proc. IEEE International Symposium on Safety, Security and Rescue Robotics (SSRR). IEEE, November 2011.

如果地面不够平坦，可以使用IMU单元对重力进行估计，然后投影到水平面建图。

pixel-accurate

像素级的扫描匹配，进一步的减少了局部的累计误差。尽管增加了计算量，但是对于闭环检测还是很有用的。该方法可以参考下面的论文：

E. Olson, “M3RSM: Many-to-many multi-resolution scan matching,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), June 2015.

一些方法对计算量进行了改进，比如提取特征点进行匹配：

F. Mart´ın, R. Triebel, L. Moreno, and R. Siegwart, “Two different tools for three-dimensional mapping: DE-based scan matching and feature-based loop detection,” Robotica, vol. 32, no. 01, pp. 19–41, 2014.

还有一些方法基于直方图的匹配方式进行闭环检测，比如：

M. Himstedt, J. Frost, S. Hellbach, H.-J. Bohme, and E. Maehle, ¨ “Large scale place recognition in 2D LIDAR scans using geometrical landmark relations,” in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 5030– 5035.

还有一些闭环检测是基于特征检测并结合机器学习，比如：

K. Granstrom, T. B. Sch ¨ on, J. I. Nieto, and F. T. Ramos, “Learning to ¨ close loops from range data,” The International Journal of Robotics Research, vol. 30, no. 14, pp. 1728–1754, 2011.

粒子滤波和图优化

粒子滤波中每个粒子必须都携带整个系统地图，对于基于网格的方式，如果地图很大的话，将会消耗大量的资源。

图优化中，根据观测结果生成约束，可以利用不同的优化方法对所有的约束进行优化，来减少约束带来的误差。

三. 系统总览

cartographer可以构建一个5cm分辨率的2D栅格地图，在移动过程中实时构建地图，雷达扫描数据(scans)经过位置估计后被插入子图(submap)，这个过程在短时间内被认为精度足够高。扫描匹配只发生在最近的子图，所以它依赖于附近的扫描数据，这个过程会产生累计误差。

为了解决累计误差，我们定期运行位姿优化。当一个子图完成之后，就不会再有新的扫描数据插入，这个子图将被加入到闭环检测当中。所有的子图和扫描数据（submaps and scans）将自动加入到闭环检测。如果待检测的值距离现在的位姿估计足够近，则扫描匹配器将会在submap中找出相应的scan，如果找到了，将会增加一个闭环约束到优化问题中。

所以闭环的扫描匹配必须比新扫描数据加入得更快一些（被加进来了，还怎么识别相同位置?），文章中加入了分支定界法和几个预先计算好的网格来实现这个功能。

四. 局部SLAM

局部slam和全局slam都是优化位姿pose：ξ=(ξx,ξy,ξθ)ξ = (ξ_x, ξ_y, ξ_θ)ξ=(ξx,ξy,ξθ)。位姿表示激光观测值的平移和旋转，也是指scans。在3D中我们还会增加imu进行重力估计，来进行2D的投影。

在局部的扫描匹配中，每个scan都会对子图MMM进行匹配，并且使用非线性优化方法使得scan和submap对齐。在扫描匹配过程中会积累误差，误差将会在全局方法中解决。

4.1 Scans

scan的原点在0∈R20 ∈ R^20∈R2，那么scan的每个点坐标为H=hkk=1,...,K,hk∈R2H ={h_k}_{k=1,...,K}, h_k ∈ R^2H=hkk=1,...,K,hk∈R2。假设scan坐标系的位姿ξξξ在submap坐标系表示为TξT_ξTξ，其实也就是将scan points从scan坐标系转换到submap坐标系：

4.2 submaps

连续的scans组成了一个submap，子图由概率网格组成M：rZ∗rZ→[pmin,pmax]M：rZ*rZ\rightarrow [p_{min},p_{max}]M：rZ∗rZ→[pmin,pmax]，其中分辨率r=5cmr=5cmr=5cm。概率小于pminp_{min}pmin则认为无障碍物，大于pmaxp_{max}pmax则认为有障碍物，在之间则认为未知。定义网格中的所有点为pixelpixelpixel。

对于每一个添加到概率网格的scan都会计算hitshitshits和missesmissesmisses集合。例如网格点xxx已经被观测到了，则：

missesmissesmisses也会有同样的等式。

4.3 ceres scan matching

将scan插入子图之前，scan的位姿pose ξξξ都会使用ceres的扫描匹配器进行优化。扫描匹配器的作用是使得scan位姿在子图中的概率最大，这是一个非线性优化问题：

其中TξT_ξTξ是hkh_khk从scan坐标系到子图坐标系的变换，这个和scan位姿坐标系变换是一样的。 Msmooth:R2→RM_{smooth} :R^2 → RMsmooth:R2→R是一个局部子图的概率值的平滑形式。这里我们使用双三插值。

这个平滑函数的数学优化往往比网格的精度更高。因为这是一个局部优化的问题，所以我们需要一个很好的初始估计。imu得出的角速度可以很好的估计扫描匹配中的旋转角θθθ。在没有imu的情况下，高频的扫描匹配或者pixelpixelpixel-accurateaccurateaccurate扫描匹配方法也可以使用。

五. 回环

因为scans仅仅匹配包含了一些scans的submap，在短时间内被认为误差足够小，但是在随着建图的进行误差也会慢慢变大。这里我们使用SPA(Sparse Pose Adjustment)的方法对scans和submaps的位姿进行优化，这个方法可以查看以下论文：

K. Konolige, G. Grisetti, R. Kummerle, W. Burgard, B. Limketkai, ¨ and R. Vincent, “Sparse pose adjustment for 2D mapping,” in IROS, Taipei, Taiwan, 10/2010 2010.

被插入的scans的相对位姿被保存(stored in memory)下来用于闭环优化。扫描匹配器在后台运行，一旦找到了闭环，相应的相对位姿将被加入到闭环优化问题中。

5.1 优化问题

闭环优化问题也是一种非线性最小二乘问题，每过几秒，我们使用ceres计算下面的值：

其中：
Ξm=ξimi=1,...,mΞ^m = {ξ^m_i}_{i=1,...,m}Ξm=ξimi=1,...,m：submap在世界坐标系的位姿。
Ξs=ξjsj=1,...,mΞ^s = {ξ^s_j}_{j=1,...,m}Ξs=ξjsj=1,...,m：scan在世界坐标系的位姿。
ξijξ_{ij}ξij：submap和scan的约束。
ΣijΣ_{ij}Σij：协方差矩阵。
对于scan jjj和submap iii，ξijξ_{ij}ξij描述了submap坐标系下的scan扫描匹配的位姿。协方差矩阵可通过下面的论文计算，也可以通过Ceres的（CS）协方差估计功能。

E. B. Olson, “Real-time correlative scan matching,” in Robotics and Automation, 2009. ICRA’09. IEEE International Conference on. IEEE, 2009, pp. 4387–4393.

残差EEE通过下面的公式计算：

损失函数ρ\rhoρ，例如HuberlossHuber\ lossHuber loss,它的作用是，当扫描匹配加入错误的约束到优化问题时，可以减小SPA中的离群值( outliers)影响。例如办公室的对称环境。离群值的替代方案包括下面论文中的方法：

P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss, and W. Burgard, “Robust map optimization using dynamic covariance scaling,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp. 62–69.

5.2 分支定界扫描匹配

在优化的时候，我们使用pixaccuratepix\ accuratepix accurate的方式进行匹配搜索，WWW表示搜索框，MnearestM_{nearest}Mnearest表示最近的网格数据。

搜索匹配的质量也可以通过(CS)进行优化。

在这里搜索的角度步长用δθδ_θδθ表示，扫描点之间的最大距离dmaxd_{max}dmax不大于分辨率rrr,使用cos法则，我们得到：

对搜索框（linear和angular的搜索框）进行积分，比如搜索框大小为Wx=Wy=7mW_x=W_y=7mWx=Wy=7m,Wθ=30oW_θ=30^oWθ=30o。其中：

这样我们就得到了一个在中心位置估计ξ0ξ_0ξ0附近的有限搜索集WWW：

当然我们可以对BBS进行暴力搜索，但是这个算法太慢了。这个算法如下：

我们这里不使用Algorithm 1，使用的是分支定界法对ξ∗ξ^*ξ∗进行计算，算法步骤参考Algorithm 2。这个算法是在下面的论文被第一次提出的：

A. H. Land and A. G. Doig, “An automatic method of solving discrete programming problems,” Econometrica, vol. 28, no. 3, pp. 497–520, 1960.

主要思想：树中的节点表示可能性的子集，根节点表示所有可能性，子节点是父节点的一部分。只要保证在节点c的内部，score(c)score(c)score(c)是它的得分上限，只要一个节点有界，那么就不存在比父节点更好的子节点。
下面介绍具体流程：

1. 节点选择

本文使用深度优先搜索算法（DFS,depth-first search）,算法的效率取决于树修建的部分，这主要依赖于两部分：好的上界和好的解决方案。这里的解决方案为DFS，它可以快速的对叶节点进行评估。为了不增加坏的匹配约束，我们引进了分数阈值(score threshold)。DFS的分支节点扫描匹配算法如Argorithm 3.

2. 分支规则

每个节点都由证书数组构成：c=(cx,cy,cθ,ch)∈Z4c = (c_x, c_y, c_θ, c_h) ∈ Z^4c=(cx,cy,cθ,ch)∈Z4。节点的高度为chc_hch有2ch×2ch2^{c_h} ×2^{c_h}2ch×2ch的旋转可能：

叶子节点的高度ch=0c_h=0ch=0，并且对应的可行解为W∋ξc=ξ0+(rcx,rcy,δθcθ)W \ni ξ_c = ξ_0 + (rc_x, rc_y, δ_θc_θ)W∋ξc=ξ0+(rcx,rcy,δθcθ)。

在算法3中，包含所有可行解的根节点没有显示，而是出现在高度为h0h_0h0的初始节点C0C_0C0中，它的搜索窗口为：

对于任意一个高度ch>1c_h>1ch>1的节点ccc，都会有四个高度为ch−1c_h-1ch−1的孩子节点。

3. 计算上界
在内部计算上界的方法：

为了最大化效率，我们使用预计算网格MprecompchM_{precomp}^{c_h}Mprecompch，同时Wc−\overset{-}{W_c}Wc−也会被取代。

cartographer：论文阅读(Real-Time Loop Closure in 2D LIDAR SLAM)相关推荐

cartographer算法(二)—— cartographer论文精读 for Real-Time Loop Closure in 2D LIDAR SLAM
Real-Time Loop Closure in 2D LIDAR SLAM文章精读原文:W. Hess, D. Kohler, H. Rapp, and D. Andor, Real-Time ...
分枝定界图解（含 Real-Time Loop Closure in 2D LIDAR SLAM论文部分解读及BB代码部分解读）
分枝定界图解网上对分枝定界的解读很多都是根据这篇必不可少的论文<Real-Time Loop Closure in 2D LIDAR SLAM>来的. 分枝定界是一种深度优先的树形搜索方 ...
Google Cartographer 《Real-Time Loop Closure in 2D LIDAR SLAM》翻译 (中英对照）
Google Cartographer <Real-Time Loop Closure in 2D LIDAR SLAM>翻译 (中英对照) 作者: Wolfgang Hess, Damo ...
real-time loop closure in 2D LIDAR SLAM 论文整理
1.cartographer整体: 主要论文: Real-Time Loop Closure in 2D LIDAR SLAM , ICRA 2016 Efficient Sparse Pose Ad ...
Real-Time Loop Closure in 2D LIDAR SLAM 翻译和总结（一）
Hess W , Kohler D , Rapp H , et al. Real-Time Loop Closure in 2D LIDAR SLAM[C]// 2016 IEEE Internati ...
google cartographer的论文《real-time loop closure in 2D LIDAR SLAM》翻译
cartographer论文翻译初衷因为自己看了两天论文毫无头绪,就想着那就先把这篇论文翻译一下,然后加上自己的一些理解.看看能不能得出点啥来. 摘要便携式激光测距仪(也就是LIDAR)以及实时 ...
【那些年我们一起看过的论文】之《Real-Time Loop Closure in 2D LIDAR SLAM》
/* 前言:新入手2d lidar一枚,打算基于cartography进行后续开发,先对论文做一定了解. */ Step one: 先全文过一遍.有网友提供翻译结果,自己对照看.了解梗概. link: ...
每日论文打卡十 LCDNet: Deep Loop Closure Detection for LiDAR SLAM based on Unbalanced Optimal Transport
论文题目 LCDNet: Deep Loop Closure Detection for LiDAR SLAM based on Unbalanced Optimal Transport arxiv ...
【读论文】Loop Closure Detection for Visual SLAM Systems Using Convolutional Neural Network
[读论文]Loop Closure Detection for Visual SLAM Systems Using Convolutional Neural Network 发表于2017年,作者是南 ...

cartographer：论文阅读(Real-Time Loop Closure in 2D LIDAR SLAM)