论文题目：CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure
中文题目：CT-ICP:实时弹性激光雷达里程计与回环检测
作者：Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet and Francois Goulette
作者机构：巴黎高等矿业大学机器人技术中心 Kitware计算机视觉团队
论文链接：https://arxiv.org/pdf/2109.12979.pdf
论文实验链接：https://github.com/jedeschaud/ct_icp
论文开源代码：https://github.com/Kitware/pyLiDAR-SLAM
本文主要是提出了一种新的实时激光雷达里程测量方法，称为CT-ICP(连续时间ICP)，通过新颖的环路检测程序完成完整的SLAM。该方法的核心是在扫描匹配中引入连续性和扫描间不连续的组合。它允许在配准过程中扫描的弹性变形，以提高精度，并增加了对不连续的高频运动的鲁棒性。在七个数据集上进行了测试:KITTI、KITTI-raw、KITTI-360、KITTICARLA、ParisLuco、Newer College和NCLT，分别用于驾驶和高频运动场景。
1 引言
本文提出的目的：
- 使用匀速运动假设矫正点云畸变的方法无法适应大幅度的方向变化或者剧烈速度变化
- 即使目前有工作考虑了连续时间内的运动，但是也无法适应高频的剧烈运动情况，或者以精度损失为代价。
本文主要贡献：
- 提出了一种基于位姿扫描内连续性和扫描间不连续的弹性激光雷达里程计。
本文次要贡献：
- 提出了一种基于密集点云的局部地图，存储在稀疏体素结构中，以获得实时处理速度。
- 在驾驶和高频运动场景的7个数据集上进行大规模实验，所有实验都有开源代码可以复现。
- 结合姿态图后端构建了完整SLAM的快速回环检测方法，为pyLiDAR-SLAM(开源)。
2 算法概述
算法主要流程如下图所示：
图中的彩色部分为激光雷达扫描。点云的颜色对应每个点的时间戳(蓝色表示时间戳更远的点云，红色表示时间戳更近的点云)。
通过在每一帧扫描的开始和结束时刻联合优化两个姿势，并根据时间戳进行插值，使扫描进行弹性变形以与地图(白点)对齐，从而创建连续时间扫描到地图的里程计。图片最下面下面说明轨迹具有扫描内姿势的连续性和扫描之间的不连续性。
3 算法框架
3.1 里程计公式
在激光雷达的每一帧中，里程计有两个位姿用于参数化插值：
- 起始位姿 Ω b ( R b , t b ) Ω_b(R_b,t_b) Ωb(Rb,tb)
- 结束位姿 Ω e ( R e , t e ) Ω_e(R_e,t_e) Ωe(Re,te)
对于在每一帧扫描的第一个时间戳 τ b \tau_b τb和最后一个时间戳 τ e \tau_e τe之间的时间 τ \tau τ∈[ τ b \tau_b τb, τ e \tau_e τe]捕获的每个传感器的测量，通过在扫描的起始和结束两个姿态之间插值来估计传感器的姿态。
与其他里程计不同的是，当前帧的起始位姿不等于上一帧的结束位姿，这两个位姿之间加入了一个邻近约束，迫使两个姿态保持接近，这使里程计对传感器的高频运动更加稳健。
本文的优化函数如下：
a
r
g
m
i
n
F
I
C
P
(
X
)
+
β
l
C
l
o
c
(
X
)
+
β
v
C
v
e
l
(
X
)
argmin F_{ICP}(X)+\beta_l C_{loc}(X)+\beta_v C_{vel}(X)
argminFICP(X)+βlCloc(X)+βvCvel(X)
其中，优化变量
X
=
(
τ
b
,
τ
e
)
∈
S
E
(
3
)
2
X=(\tau_b,\tau_e) \in SE(3)^2
X=(τb,τe)∈SE(3)2,
F
I
C
P
F_{ICP}
FICP为扫描到地图的连续时间ICP(其实就是插值和ICP的过程)。
F
I
C
P
(
X
)
=
1
∣
I
n
∣
∑
i
∈
I
ρ
(
r
i
2
∣
X
∣
)
F_{ICP}(X) = \frac{1}{|\mathbb{I}^n|}\sum_{i \in \mathbb{I}} \rho (r^2_i|X|)
FICP(X)=∣In∣1i∈I∑ρ(ri2∣X∣)
I
\mathbb{I}
I是从每一帧雷达数据中提取的一个特征点序号的集合
I
n
:
{
p
i
∈
S
n
∣
i
∈
I
}
\mathbb{I}^n: \{p_i \in \mathbb{S^n} | i \in \mathbb{I} \}
In:{pi∈Sn∣i∈I}，对于每个i有：
r
i
[
X
]
=
a
i
(
p
i
W
[
X
]
−
q
i
W
)
⋅
n
i
r_i[X]=a_i(p^W_i[X]-q^W_i) · n_i
ri[X]=ai(piW[X]−qiW)⋅ni
p
i
W
[
X
]
=
R
α
i
[
X
]
∗
p
i
L
+
t
]
a
l
p
h
a
i
[
X
]
p^W_i[X]=R^{\alpha_i}[X]*p^L_i+t^{]alpha_i}[X]
piW[X]=Rαi[X]∗piL+t]alphai[X]
R
α
i
[
X
]
=
s
l
e
r
p
(
R
b
,
R
e
,
α
i
)
R^{\alpha_i}[X]=slerp(R_b,R_e,\alpha_i)
Rαi[X]=slerp(Rb,Re,αi)
t
α
i
[
X
]
=
(
1
−
α
i
)
t
b
+
α
i
t
e
t^{\alpha_i}[X]=(1-\alpha_i)t_b+\alpha_it_e
tαi[X]=(1−αi)tb+αite
-
r i r_i ri是样本点 p i W p^W_i piW与其在地图中最近的邻居之间点到平面距离的残差；
-
p i W p^W_i piW为世界坐标系中表示的点 p i p_i pi， n i n_i ni为 p i W p^W_i piW在局部地图中邻域的法线， p i l p^l_i pil为传感器测量值(在LiDAR坐标系中)；
-
Ω α i [ X ] = ( R α i , t α i ) ∈ S E ( 3 ) Ω^{\alpha_i}[X]=(R^{\alpha_i},t^{\alpha_i})∈SE(3) Ωαi[X]=(Rαi,tαi)∈SE(3)是雷达坐标系在时间 τ i \tau_i τi到世界W的变换。通过定义 α i = ( τ i − τ b ) / ( τ e − τ b ) \alpha_i=(\tau_i-\tau_b)/(\tau_e-\tau_b) αi=(τi−τb)/(τe−τb)，在 Ω b Ω_b Ωb和 Ω e Ω_e Ωe之间进行插值估计。对于旋转插值，使用标准球面线性插值(slerp)。
-
还引入了有利于平面邻域的权重: a i = a 2 D = ( σ 2 − σ 3 ) / σ 1 a_i=a_{2D}=(\sigma_2−\sigma_3)/\sigma_1 ai=a2D=(σ2−σ3)/σ1，是 p i W p^W_i piW邻域的平面度，其中 σ i \sigma_i σi是邻域协方差特征值的平方根。
优化函数还引入了两个约束
C
l
o
c
C_{loc}
Cloc(位置一致性约束)和
C
v
e
l
(
X
)
C_{vel}(X)
Cvel(X)(等速约束)，其权重分别为
β
l
\beta_l
βl和
β
v
\beta_v
βv，定义如下:
C
l
o
c
(
t
b
)
=
∣
∣
t
b
−
t
e
n
−
1
∣
∣
2
C_{loc}(t_b)=||t_b-t^{n-1}_e||^2
Cloc(tb)=∣∣tb−ten−1∣∣2
C
v
e
l
(
t
b
,
t
e
)
=
∣
∣
(
t
e
−
t
b
)
−
(
t
b
n
−
1
−
t
e
b
−
1
)
∣
∣
2
C_{vel}(t_b,t_e)=||(t_e-t_b)-(t^{n-1}_b-t^{b-1}_e)||^2
Cvel(tb,te)=∣∣(te−tb)−(tbn−1−teb−1)∣∣2
C l o c C_{loc} Cloc迫使传感器的开始和结束位置保持一致(限制不连续性)，而 C v e l ( X ) C_{vel}(X) Cvel(X)限制过快的加速。
CT-ICP执行迭代，直到满足参数步长范数的阈值(通常为平移0.1 cm和旋转0.01°)或达到多次迭代(5次以确保实时性)。
3.2 局部地图和鲁棒的配置文件
作为局部地图，世界坐标系(W)中的点存储在体素的稀疏数据结构中，以便比kd-trees(常数时间访问而不是对数访问)更快地进行邻域访问。
地图的体素大小控制着邻域搜索的半径，以及存储点云的详细程度。
每个体素最多存储20个点，这样两个点之间的距离就不会超过10厘米，以限制由于沿着扫描线的测量密度而造成的冗余。
一旦一个体素被填满，就不会再插入点了。
为了构建点 p i W p^W_i piW的邻域(用于计算ni和ai)，从当前点的27个相邻体素中选择地图中k=20个最近邻。在对当前扫描n运行CT-ICP后，这些点被添加到局部地图中。
使用这些类型地图的里程计对错误配准高度敏感，并且无法从错误扫描插入的地图污染中恢复，为了处理这个问题，引入了一个鲁棒的配置文件，可以检测硬情况(快速方向变化)和注册失败(位置不一致或大量新关键点落在空体素中)，并尝试使用一组更保守的参数(最明显的是大量采样关键点和更大的邻域搜索)对当前扫描进行新的注册;对于重要的方向修改(≥5◦)，不会在地图中插入新的扫描，因为这有更高的不对齐概率。
3.3 回环检测和后端
回环检测算法在其内存中保留了一个由里程计记录的最后扫描的窗口。当窗口达到 N m a p N_map Nmap扫描的大小时，这些点被聚合成一个点云，放置在窗口中心的坐标框架中。
然后将该地图的每个点插入到二维高程网格中，使每个像素点保持最大高程。从这个二维网格中，通过在 Z m i n Z_min Zmin和 Z m a x Z_max Zmax之间剪切每个像素的z坐标来获得高程图像。
然后提取旋转不变的2D特征，并将其与高程网格一起保存在内存中。除了最后一次 N o v e r l a p N_overlap Noverlap之外的所有扫描都将从窗口中删除。
每次建立新的高程图像时，它都会与保存在其内存中的高程图像进行匹配。使用RANSAC鲁棒拟合两个特征集之间的二维刚性变换，并使用内层数的阈值来验证对应关系。当匹配被验证后，对初始2D变换在高程网格的点云上进行ICP细化，产生精确的6自由度闭环约束。
后端使用位姿图，当添加新的里程约束时，会向图中添加新的姿态，但只有当检测到新的环路约束时，轨迹才会进行全局优化，此时闭环模块的轨迹也会更新。
4 实验与结果
在KITTI, KITTI-raw, KITTI-360, KITTI- carla, ParisLuco, Newer College Dataset (NCD)和NCLT数据集上进行了实验。
4.1 里程计实验
使用KITTI相对平移误差(RTE)进行评估。并且在提供的数据集上与其他三种LiDAR里程计进行比较:MULLS、IMLSSLAM和pyLIDAR-SLAM F2M。实验结果如下：
4.2 回环检测实验
为了定量评价其质量，RTE不太适合，并且在环路关闭后趋于恶化。为了证明全局轨迹改进，使用标准的绝对轨迹误差(ATE)来进行评估。然而，为了将质量评估与初始姿态的方向误差分离开，在计算ATE之前首先估计地面真值与估计轨迹之间的最佳刚性变换。
下图给出了实验定性结果：
下表给出了实验定量结果：
最后给出了NCLT数据集(左上)、KITTI-CARLA(右上)、Newer College数据集(左下)和ParisLuco(右下)的聚合点云使用CT-ICP获得的地图的质量。
5 总结
本文提出了一种新的实时激光雷达里程计方法，该方法在七个不同数据集(从驾驶到高频运动场景)上超越了目前的技术水平。该方法的核心是匹配CT-ICP的连续扫描，它在优化过程中弹性地扭曲新的扫描，以补偿采集过程中的运动。
未来的工作将专注于改进后端，将所提出的连续公式扩展到扫描匹配之外，并充分利用循环关闭程序。
![[CKA]考试之检查可用节点数量](https://img-blog.csdnimg.cn/9b0b748b5f6645efa58455eae291736c.png)
