LOAM: Lidar Odometry and Mapping in Real-time
本文主要是对loam论文的一些学习总结，如有不对之处，希望大家提出宝贵的意见，共同进步~
文章摘要
作者提出了一种实时的里程计和建图方法，该方法使用在6个*度上移动的2轴激光雷达测量距离（作者使用单线激光雷达自制）。由于距离测量值的不同时性和运动畸变会导致点云的错误配准，所以这个问题的解决还是非常困难。在作者这篇文章之前，连续的3D地图一般是离线批量处理，使用回环来校正随时间而产生的漂移。作者的方法不需要高精度的距离和惯导的测量就可以实现低漂移和低计算量，其关键就在于将同时定位和建图问题一分为二，通过两种算法进行优化。第一种算法执行高频率（10HZ）但是低精度的里程计去估计雷达速度。另一种方法就是以比第一种算法低一个数量级的频率（1HZ）执行精细的点云配准。通过以上两种方法就可以实现实时的定位与地图构建了。该方法已经通过大量实验以及KITTI里程计基准进行了评估。结果表明，该方法可以达到离线批处理方法的水平。
如下图所示：最左侧体现了运动过程的点云失真，Lidar Odometry估计雷达速度并进行校准，Lidar Mapoing 实现点云配准以创建地图。
一、简介
作者先分析了当前的激光点云存在的问题，并为此提出了解决的办法，即上述的两种算法，具体来说，真正的创新点在于两种算法都提取了点云的边缘点和平面点作为特征点，并匹配到相应的线段或者平面块上。在里程计算法中，通过确保快速计算来找到特征点的对应关系。而在建图算法中，通过相关的特征向量和特征值来检查局部地图立方体的几何分布来确定对应关系。
分解原始的问题，首先在线运动估计（里程计）这一简单问题很容易解决。然后，建图问题通过批量优化的方法以满足高精度的要求。并行算法结构保证了实时性。此外，由于里程计比建图频率更高，所以建图有足够的时间实现高精度，当以较低的频率运行时，建图算法处理了大量的特征点，并使用更多次的迭代进行收敛。
二、相关工作
如果激光雷达的转速远高于物体的速度，那么运动畸变可以忽略不计。可是实际情况却并不是这样，尤其像作者使用的两轴转动的雷达，运动畸变无法忽略。当然如果有其他传感器辅助的情况下，这个问题的解决有更多的方法，但是仅对于3Dslam而言，作者假设了匀速运动模型减小运动畸变。
关于运动畸变的补充：
以3D激光雷达的某一线为例，假设激光雷达逆时针旋转，当物体静止不动时，该线激光扫过的环境如左图所示，当假设物体以一定速度向前行驶时，激光雷达观测的环境信息如右图蓝线所示，但是实际上的环境应该如红线所示，这就是运动畸变。其原因主要是激光点是在不同时间、不同地点接受的，先接受的点，距离比当前环境的实际距离要长，产生的误差也就越大。
三、符号和任务描述
本文使用上角大写字母表示坐标系，sweep表示3D雷达扫描一圈，k表示第几次扫描周期，
P
k
P_k
Pk表示第k次扫描所产生的点云信息。同时定义如下两个坐标系：
1.激光雷达坐标系 {L} 是一个3D坐标系，其原点位于激光雷达的几何中心。x轴指向左侧，y轴指向上方，z轴指向前方。在{
L
k
L_k
Lk}z中，点i (
i
∈
P
k
i \in P_k
i∈Pk) 的坐标可以表示为
X
(
k
,
i
)
L
X^L_{(k,i)}
X(k,i)L。
2. 世界坐标系{W}是与初始位置{L}重合的三维坐标系。{Wk}中点i (
i
∈
P
k
i \in P_k
i∈Pk) 的坐标可以表示为
X
(
k
,
i
)
W
X^W_{(k,i)}
X(k,i)W。
所以所需解决的任务可以描述为：给定一系列激光雷达云
P
k
P_k
Pk，
k
∈
Z
+
k\in Z^{+}
k∈Z+，计算激光雷达在第k次扫描期间的自我运动，并使用
P
k
P_k
Pk构建地图。
四、系统概述
如下图所示，
P
^
\hat{P}
P^是在
L
L
L坐标系下表示的接受的激光点，在第k个sweep周期内拼接为
P
k
P_k
Pk。然后
P
k
P_k
Pk被之间提到的两个算法开始处理。激光里程计获取点云并计算激光雷达在两次连续的sweep中的相对运动,估计出的运动用于运动畸变的处理,整个算法的运行频率10HZ。里程计的输出被进一步被建图部分处理。以1HZ的频率将未失真的点云加入到地图中。最后将两种算法融合得到的相对位姿以10HZ的频率发布。
五、激光里程计
5.1 特征提取
在边缘线或者平面块中选取特征点。假设
i
i
i是
P
k
P_k
Pk中的一个点，
S
S
S是激光雷达在同一次扫描中包含
i
i
i的连续点的集合，
i
i
i在序列的中间，左右两边各一半点云。为形容平面的光滑度，于是定义
c
c
c如下:
关于平滑度
c
c
c的补充：
其实很显然就是拿选中的点距离与周围其他点的距离做差，再做归一化。若是在一个平面上，相邻点的距离差值肯定比较小。若是处在边缘线上（不在一个平面），则两点之间的距离差则比较大。
(我看很多文章都称
c
c
c为曲率，但是实际上文章并没有说明，并且这个公式也不是曲率公式)
扫描中的点根据 c c c值排序，然后选择具有最大 c c c值(即边缘点)和最小 c c c值(即平面点)的特征点。为了在环境中均匀分布特征点，我们将一个扫描周期拼接成的点云分成四个相同的子区域。每个子区域最多可以提供2个边缘点和4个平面点。只有当点 i i i的 c c c值大于或小于阈值，并且所选点数不超过最大值时，点 i i i才能被选为边缘点或平面点。
在选择特征点时，我们希望避免选择被已选择的特征点包围的，或者与激光束大致平行的局部平面上的点(图(a)中的点B)。这些观点通常被认为是不可靠的。此外，我们希望避开遮挡区域边界上的点。一个例子如图(b)所示。点A是激光雷达云中的边缘点，因为它的连接表面(虚线段)被另一个对象遮挡。但是，如果激光雷达移动到另一个视点，遮挡区域可能会改变并变得可见。为了避免上述要选择的点，我们再次找到点S的集合。只有当点
S
S
S不是大致平行于激光束的平面，并且在点
S
S
S中没有点在激光束的方向上通过间隙与点I断开并且同时更靠近激光雷达点
i
i
i(例如图(b)中的点B)时，点
i
i
i才能被选择。
总之，特征点被选择为从最大c值开始的边缘点，以及从最小c值开始的平面点，并且如果选择了一个点,那么要满足：
1.所选边缘点或平面点的数量不能超过子区域的最大值。
2.它周围的点都没有被选中。
3.它不能在大致平行于激光束的平面上，也不能在遮挡区域的边界上。
5.2 特征匹配
里程计算法估计激光雷达在一个sweep内的运动。假设
t
k
t_k
tk是第
k
k
k次sweep的开始时间。在每次扫描结束时，扫描过程中感知到的点云
P
k
P_k
Pk被重新投影到时间戳
t
k
+
1
t_{k+1}
tk+1,如下图所示：
把投影后的点云记为
P
k
ˉ
\bar{P_k}
Pkˉ，在第
k
+
1
k+1
k+1次sweep中，
P
k
ˉ
\bar{P_k}
Pkˉ与新一帧点云
P
k
+
1
P_{k+1}
Pk+1一起估计雷达的相对运动。设
ε
k
+
1
\varepsilon _{k+1}
εk+1和
H
k
+
1
H_{k+1}
Hk+1分别是
P
k
+
1
P_{k+1}
Pk+1中边缘点和平面点的集合。我们将从
P
k
ˉ
\bar{P_k}
Pkˉ 中找到边缘线作为
ε
k
+
1
\varepsilon _{k+1}
εk+1中点的对应，找到平面区域作为
H
k
+
1
H_{k+1}
Hk+1中点的对应。
在每一次迭代中，
ε
k
+
1
\varepsilon _{k+1}
εk+1和
H
k
+
1
H_{k+1}
Hk+1通过当前的估计被重新投影到该sweep的起点，记作
ε
~
k
+
1
\tilde \varepsilon _{k+1}
ε~k+1和
H
~
k
+
1
\tilde H_{k+1}
H~k+1，并找到
P
k
ˉ
\bar{P_k}
Pkˉ中最近邻的点。其中
P
k
ˉ
\bar{P_k}
Pkˉ被储存在KD-Tree中便于索引。
上图(a)表示寻找边缘点对应的边缘线的过程。假设
i
∈
ε
~
k
+
1
i \in \tilde \varepsilon _{k+1}
i∈ε~k+1。而边缘线
i
i
i和
j
j
j由两点表示，其中
j
j
j为
P
k
ˉ
\bar{P_k}
Pkˉ中与
i
i
i最近邻的点，而
l
l
l为相邻两次scan中与
i
i
i最近邻的点。
i
i
i和
j
j
j显然不是一个scan得到的，一次scan中只能包含边缘线的一个点，否则就不是边缘线了。
上图(b)示出了寻找平面点的对应的平面的过程。假设
i
∈
H
~
k
+
1
i \in \tilde H_{k+1}
i∈H~k+1。平面由三个点表示。类似于最后寻找边缘线，我们在中
P
k
ˉ
\bar{P_k}
Pkˉ找到
i
i
i的最近邻，表示为
j
j
j。然后，我们找到另外两个点，
l
l
l和
m
m
m，
l
l
l是和
j
j
j在同一scan中与
i
i
i最近的点，而
m
m
m表示在与
j
j
j相邻的scan中与
i
i
i最近的点。这样可以保证三个点不共线。
找到特征点的对应关系后，现在我们导出表达式计算从特征点到其对应关系的距离。在下一节中，我们将通过最小化特征点的总距离来恢复激光雷达运动。我们从边缘点开始。对于
i
∈
ε
~
k
+
1
i \in \tilde \varepsilon _{k+1}
i∈ε~k+1，如果
(
i
,
l
)
(i,l)
(i,l)是对应的边缘线，
j
,
l
∈
P
~
k
j,l \in \tilde P_{k}
j,l∈P~k，则点到直线的距离可以计算如下:
对于点
i
∈
H
k
+
1
i \in H_{k+1}
i∈Hk+1，如果
(
j
,
m
,
l
)
(j,m,l)
(j,m,l)是对应的平面，
j
,
l
∈
P
~
k
j,l \in \tilde P_{k}
j,l∈P~k，点到平面的距离是:
关于sweep和scan的补充：
一般的，对于多线雷达而言，所有线束旋转一周称之为sweep，而其中某一线束旋转一周则称为scan。
5.3 运动估计
激光雷达运动在扫描过程中以恒定的角速度和线速度进行建模。这允许我们在不同时间接收的点的扫描内线性插值姿态变换。假设
t
t
t是当前时间戳，
t
k
+
1
t_{k+1}
tk+1是第k+1次sweep的开始时间。设
T
K
+
1
L
T^L_{K+1}
TK+1L为
[
t
k
+
1
,
t
]
[t_{k+1},t]
[tk+1,t]之间的lidar姿态变换。
T
K
+
1
L
T^L_{K+1}
TK+1L包含激光雷达在6-DOF中的刚性运动，
T
K
+
1
L
=
[
t
x
,
t
y
,
t
z
,
θ
x
,
θ
y
,
θ
z
]
T
T^L_{K+1}=[t_x,t_y,t_z,\theta_x,\theta_y,\theta_z]^T
TK+1L=[tx,ty,tz,θx,θy,θz]T，其中
t
x
,
t
y
,
t
z
t_x,t_y,t_z
tx,ty,tz分别沿
L
L
L的
x
,
y
,
z
x,y,z
x,y,z的轴平移，而旋转角度
θ
x
,
θ
y
,
θ
z
\theta_x,\theta_y,\theta_z
θx,θy,θz遵循右手定则。给定点
i
∈
P
k
+
1
i \in P_{k+1}
i∈Pk+1，其时间戳为
t
i
t_i
ti，而
T
(
k
+
1
,
i
)
L
T^L_{(k+1,i)}
T(k+1,i)L是
[
t
k
+
1
，
t
i
]
[t_{k+1}，t_i]
[tk+1，ti]之间的姿态变换。
T
(
k
+
1
,
i
)
L
T^L_{(k+1,i)}
T(k+1,i)L可以通过
T
K
+
1
L
T^L_{K+1}
TK+1L的线性插值来计算:
回想一下，
ε
k
+
1
\varepsilon _{k+1}
εk+1和
H
k
+
1
H _{k+1}
Hk+1是从
P
k
+
1
P_{k+1}
Pk+1提取的边缘点集和平面点集，
ε
~
k
+
1
\tilde \varepsilon _{k+1}
ε~k+1和
H
~
k
+
1
\tilde H _{k+1}
H~k+1是重新投影到sweep开始时刻
t
k
+
1
t_{k+1}
tk+1的点集。为了求解激光雷达运动，我们需要在
ε
k
+
1
\varepsilon _{k+1}
εk+1和
ε
~
k
+
1
\tilde \varepsilon _{k+1}
ε~k+1之间，或者
H
k
+
1
H_{k+1}
Hk+1和
H
~
k
+
1
\tilde H _{k+1}
H~k+1之间建立几何关系。使用上图的变换，我们可以导出：
其中
X
(
k
+
1
,
i
)
L
X^L_{(k+1,i)}
X(k+1,i)L是
ε
k
+
1
\varepsilon _{k+1}
εk+1或
H
k
+
1
H _{k+1}
Hk+1中点
i
i
i的坐标，
X
~
(
k
+
1
,
i
)
L
\tilde X^L_{(k+1,i)}
X~(k+1,i)L是
ε
~
k
+
1
\tilde \varepsilon _{k+1}
ε~k+1或
H
~
k
+
1
\tilde H _{k+1}
H~k+1的对应点，
T
(
k
+
1
,
i
)
L
(
a
:
b
)
T^L_{(k+1,i)} (a : b)
T(k+1,i)L(a:b)是
T
(
k
+
1
,
i
)
L
T^L_{(k+1,i)}
T(k+1,i)L的第a至第b个条目，并且R是由罗德里格斯公式定义的旋转矩阵:
在上式中，θ是旋转角：
ω是代表旋转方向的单位向量:
ω
^
\hat{\omega}
ω^是ω的斜对称矩阵。
回想一下计算
ε
~
k
+
1
\tilde \varepsilon _{k+1}
ε~k+1和
H
~
k
+
1
\tilde H _{k+1}
H~k+1中的点之间的距离及其对应关系。结合(2)和(4)-(8)，我们可以导出
ε
k
+
1
\varepsilon _{k+1}
εk+1中的边缘点和对应边缘线之间的几何关系:
类似地，我们可以在
H
k
+
1
H _{k+1}
Hk+1中的平面点和对应的平面之间建立另一种几何关系:
最后，我们用LM方法求解激光雷达运动。对
H
k
+
1
H _{k+1}
Hk+1和
ε
k
+
1
\varepsilon _{k+1}
εk+1中的每个特征点距离函数相加，我们得到一个非线性函数：
其中
f
f
f的每一行对应于一个特征点，
d
d
d包含相应的距离。我们计算
f
f
f相对于
T
k
+
1
L
T^L_{k+1}
Tk+1L的雅可比矩阵，表示为
J
J
J，其中
J
J
J = ∂
f
f
f /∂
T
k
+
1
L
T^L_{k+1}
Tk+1L。然后，通过非线性迭代求解，使
d
d
d最小化
其中λ是由L-M方法确定的因子。
5.4 激光里程计算法
激光雷达里程计算法如下图所示。
六、建图
建图算法的频率低于里程计算法频率，在每次sweep中只调用一次，在第k+1次sweep结束时，里程计输出未失真的点云
P
ˉ
k
+
1
\bar P_{k+1}
Pˉk+1，同时输出姿态变换矩阵
T
k
+
1
L
T^L_{k+1}
Tk+1L,包含在
[
t
k
+
1
,
t
k
+
2
]
[t_{k+1},t_{k+2}]
[tk+1,tk+2]之间期间的激光雷达运动。
建图算法匹配并定位世界坐标{W}中的
P
ˉ
k
+
1
\bar P_{k+1}
Pˉk+1，如上图所示。为了解释这个过程，让我们定义
Q
k
Q_k
Qk为累积到第k次sweep的地图的点云，，并设
T
k
W
T^W_k
TkW是第k次sweep，
[
t
k
,
t
k
+
1
]
[t_k,t_{k+1}]
[tk,tk+1]，激光雷达相对于地图的姿态。利用激光雷达里程计的输出，建图算法将
T
k
W
T^W_k
TkW在
[
t
k
+
1
,
t
k
+
2
]
[t_{k+1},t_{k+2}]
[tk+1,tk+2]扩展，以获得
T
k
+
1
W
T^W_{k+1}
Tk+1W，并将
P
ˉ
k
+
1
\bar P_{k+1}
Pˉk+1投影到世界坐标{W}，表示为
Q
~
k
+
1
\tilde Q_{k+1}
Q~k+1。接下来，该算法将
Q
k
Q_{k}
Qk与
Q
ˉ
k
+
1
\bar Q_{k+1}
Qˉk+1相匹配，优化激光雷达姿态
T
k
+
1
W
T^W_{k+1}
Tk+1W。
特征点的提取方法与之前章相同，但使用了10倍的特征点(即里程计10次输出的数据）。为了找到特征点的对应关系，我们将点云存储在地图
Q
k
Q_{k}
Qk上，大小为10立方米。立方体中与
Q
ˉ
k
+
1
\bar Q_{k+1}
Qˉk+1相交的点被提取并存储在3D KD树中。我们在
Q
k
Q_{k}
Qk中寻找特征点周围的特定区域中的点。设
S
′
S'
S′为一组选取的周围点的集合。我们计算
S
′
S'
S′的协方差矩阵，表示为M，以及M的特征值和特征向量，分别表示为V和E。如果
S
′
S'
S′分布在一条边缘线上，V包含一个显著大于其他两个的特征值(即一大两小)，E中与最大特征值相关的特征向量代表边缘线的方向。另一方面，如果
S
′
S'
S′分布在平面上，V包含两个较大的特征值，第三个显著较小，而E中与最小特征值相关联的特征向量表示平面补片的方向。边缘线或平面的位置通过穿过
S
′
S'
S′的几何中心来确定。
为了计算从特征点到其对应点的距离，我们在一条边缘线上选择两个点，在一个平面上选择三个点。然后根据之前的计算方式进行距离计算，构建非线性函数。与之前不同的是
Q
ˉ
k
+
1
\bar Q_{k+1}
Qˉk+1中的所有点都共享时间戳
t
k
+
2
t_{k+2}
tk+2（即不需要线性插值）。再次通过LM方法求解非线性问题，并将
Q
ˉ
k
+
1
\bar Q_{k+1}
Qˉk+1匹配到地图上，为了均匀分布这些点云，通过体素滤波器缩小为5cm的立方体。
姿态变换如上图所示。蓝色区域表示激光雷达的姿态输出，
T
k
W
T^W_{k}
TkW，每次sweep生成一次。橙色区域代表频率约为10Hz的激光雷达里程计
T
k
+
1
L
T^L_{k+1}
Tk+1L的输出。激光雷达相对于地图的姿态是两种变换的组合，频率与激光雷达里程计相同。
七、实验
未完待续~