PLC-LiSLAM：线-面-圆柱体-激光SLAM（RAL 2022）

中文标题: PLC-LiSLAM：线-面-圆柱体-激光SLAM，实时+高精度，从数学角度证明解决大规模多几何残差优化实现实时性的可能性。

论文阅读：PLC-LiSLAM: LiDAR SLAM With Planes, Lines,and Cylinders(RAL 2022)

Motivation

在现实生活，平面，线和圆柱体是非常常见的结构，目前已有工作做了类似于BA的平面adjustment(即.PA)，但是这类工作在缺少平面的场景效果很差，为了增强系统的鲁棒性，引入更多的特征是有必要的，因此作者在本文中引入了额外的线和圆柱体特征，并且实现了平面-线-圆柱体-adjustment(即. PLCA)，并且证明了可以通过预处理使得PLCA独立于点云的大小，从而实现系统的实时性。

Contribution

1. 文章证明了通过一些预处理方式，局部和全局PLCA的最小迭代次数与从平面、线和圆柱体中捕获的点的数量无关，从而证明了有效解决一个大规模的PLCA问题的可行性。
2. 提出了一种有效的PLCR(R即注册，registration)解决方案。基于两次连续扫描之间有一个小的旋转的假设，采用一阶泰勒展开来近似旋转。
3. 提出的算法可以容忍一定的检测误差。因为激光雷达点云的遮挡和稀疏性，检测过程容易引入误差。在传统注册框架(LOAM&ICP)中很难解决这个问题，因为没有进行较好的数据关联。本文中的算法通过后端的代价函数来约束检测误差 )

Content

1.符号说明

a.平面

n为平面法向量，且模为1，d为坐标系原点到平面的距离，同时也可以转化为最近点参数化的形式：

b.线

应用普鲁克坐标系来表示3D线

，d是方向，m的模等于坐标系原点到直线的距离，线元素具有四个自由度，因为对于每一个直线，他的旋转矩阵可以表示为：

，将这个旋转角度表示为：

，那么最终的四自由度形式的直线可以表示为:

c.圆柱体

圆柱体可以表示为

，l是圆柱体中心线的表示，r是半径，因为线是4自由度的，所以圆柱体是5个自由度

。

d.观测

在观测中，平面线和圆柱体统一用m表示，假设

是第j个地标，并且

是在位姿

处观测到，那么在

处观测到的所有

可以形成一个

大小的点集

，将这些所有点按照齐次坐标存储，可以得到一个

x4大小的矩阵

。

e.优化

LM算法用来解决上述观测的最小二乘问题，但是因为观测得到的矩阵过于庞大，所以计算量很大。可以发现，在LM迭代过程中

，仅有

和

需要迭代给出，在后续的章节中，可以证明这两个迭代量在PLCA的过程中是具有特殊结构的，从而可以极大的加速运算。

2.点到模型的残差

a.点到面残差

对于关联到对应面(参数化为)的点

，他和对应关联面在处的残差可以定义为

b.点到线残差

同上，可以定义为：

c.点到圆柱体残差

同上，可以定义为：

，其中

是先前定义的点线残差向量。点到圆柱体残差的另一个表示形式可以是

，但是还是平方的形式有利于计算，具体的说，后续的章节将证明，通过一些预处理，平方残差的计算复杂度与点云中的点数无关。

d.固定位姿的残差

文章采取了滑窗优化的策略，滑窗外的位姿采取固定的方式，对于同一个路标，如果当前滑窗内位姿和滑窗外位姿同时观测到，那么也会相应的产生一个残差：

3.系统概述

系统设计为前端和后端。前端检测平面、直线和圆柱体，并建立局部到全局的数据关联以进行实时位姿估计，并且确定创建新关键帧的时机。后端包括局部和全局PLCA，局部PLCA优化滑窗内的位姿以及这些位姿观察到的地标，新关键帧出现时，执行局部PLCA。全局PLCA优化了除第一个位姿外的所有地标和所有位姿。当地标被重新访问时，执行全局PLCA。

4.前端

a.地标点检测

首先按照LOAM提取曲率的方式分割激光雷达扫描线:

用cm表示扫描线c值的中值。c值高于95%或大于5cm的点视为在边上。如果一些边缘点彼此接近，只保留具有最大c的边缘点。这些点形成一个集合E，然后将扫描线分割为段。段端点之间的点形成一个集合F。采用区域生长方法来检测标志点。

1)平面和圆柱体检测

从第一条扫描线开始检测。给定第i条扫描线中的一段P，对于P中的每个点，首先找到其k个最近点，这些点属于F，但不在第i条扫描线中。然后使用RANSAC将平面拟合到这些点。如果内点的平均点到面距离大于阈值或内点的数量小于阈值，尝试使用RANSAC算法将圆柱体拟合到这些点。如果圆柱体假设具备更多的内点和更小的平均距离，把这些点视为属于圆柱体的点。重复该步骤，直到所有点都被处理过。

2)线检测

对于属于第 i 条扫描线的点 p0 ∈ E，首先在第 (i + 1) 条扫描线中找到最近的点 p1 ∈ E。使用 p0 和 p1 计算线 l。然后在第 (i + 2) 条扫描线中找到 p1 的最近点 p2 ∈ E。如果 p2 和 l 之间的距离小于距离，使用 p0、p1 和 p2 拟合一条新线。重复此过程，直到无法添加任何点。

3)检测后续

检测初步结束后，合并具有相似参数的地标。只在第一次扫描时检测整个扫描中的平面、线和圆柱体。对于其他关键帧，这些地标点在非跟踪点中被检测到。下图展示了该检测算法的结果。如下图所示，在此阶段可能会错误识别地标。错误的识别会在后端进行修正

b.前向ICP流

采用前向 ICP 流来建立本地到全局的数据关联。假设 Si+1 是 Si 的后续扫描。首先计算 Si+1 的集合 E 和 F，分别为 E 和 F 构建两个 KD 树。假设 P 是 mj 在 Si 中的观测值。对于 P m ij 中的每个点，在 Si+1 中找到 n 个最近邻（在我们的实验中 n = 2）。对于平面或圆柱体，只需将这些点组合起来。对于线，只保留 c 最大的一条。假设这会产生一个集合 Q 。采用RANSAC算法找到内点P，然后通过区域增长方法扩大P。如果 |P |太小了，忽略这个匹配。否则产生局部到全局的关联。后续这些关联在 PLCR 中用于位姿估计，对于不同的地标，同时并行执行前向 ICP 流。

c.创建关键帧

新关键帧选择条件：

a)当前帧中超过 20% 的点未被跟踪。

b)当前帧与最后一个关键帧的旋转角度大于 5◦

c)当前帧与最后一个关键帧之间的距离大于阈值 t1（室内环境为 t1 = 0.2m，室外环境为 t1 = 0.5m）。

对于每个新关键帧，检测未跟踪点中的平面、线和圆柱体。然后将这些新的局部地标与全局地标匹配。对于新的检测，找到具有最小均方根误差 (RMSE) 的全局地标。用表示这个最小的 RMSE。如果 < λ（室内环境中 λ = 5 cm，室外环境中 λ = 15 cm），维持关联。如果< 3λ，执行几何一致性检查（GCC），否则添加一个新的全局地标。

通过试错法检查新对应关系的几何一致性。具体来说，将新的匹配关系添加到 PLCR、局部和全局 PLCA 中。在 PLCR 中单独检查每个新的匹配关系，并在本地和全局 PLCA 中一起检查新的匹配关系。如果在上述三个检查之一中新对应的 RMSE 小于 λ，即可保留该匹配。

滑窗中的关键帧有限，最旧的关键帧会被剔除，如果满足下述条件之一，进行保留：

a)该关键帧包含新检测到的地标

b)该关键帧与最后保留的关键帧之间的旋转角度大于 10°

c)该关键帧与最后保留的关键帧之间的距离大于 t2（实验中室内环境为 t2 = 2 m，室外环境为 t2 = 5 m）。

5.PLCA

a.全局PLCA

全局 PLCA 的形式为(第一个位姿在优化期间是固定的)：

表示残差向量，表示对应的雅可比，在LM中，实际用到的是和，这两项是具有特殊结构的。首先可以把和分块为：

基于这个分块，可以得到：

接下来直接给出

关于第j个平面，线和圆柱体的命题结论（关于命题的证明在原文的附录，感兴趣的读者可自行翻阅）。

1)命题1:

和

具备如下形式:

其中

，

，

，

2)命题2: 对于

，定义Lj=

并且

为Lj的前3行，那么点到线残差可以定义为：

其中

，

3)命题3：

和

分别类似于

和

，并且

的两个非0块具备如下的形式：

其中

,

.

的四个非0块具备如下形式：

4)命题4：点到圆柱体的残差的形式可以转变为：

5)命题5：给定

，

和

的计算复杂度是传统方法(即计算)的倍，并且因为是个常数矩阵，所以只需要计算他一次，并且可以复用。并且在给定的情况下，

和

都是独立于点的数量的。

b.局部PLCA

局部PLCA的形式如下：

类似的，存在关于局部PLCA的几个命题：

1)命题1:

和

具备如下形式：

其中

，

2)命题2: 关于观测

的残差

，具备如下形式：

其他的命题形式类似于全局PLCA，除此以外，根据这些命题，还可以得出一个增量更新

的方法，具体来说，用表示第n个从滑窗移除的关键帧，在的观测m是点集

，对于平面和线，可以从点集中计算出

，并且可以根据旧的信息构建出：

，从而根据上述的命题，可以得到：

。最后，可以得到：

其中

，

3)矫正检测误差

检测可能存在错误。常见错误是圆柱被错误地识别为线或平面，如下图所示，

另一个错误是由于遮挡导致的错误线，如下图所示。

在本地和全局 PLCA 期间，检查每个地标的 RMSE。如果 RMSE 大于阈值，则可能存在错误。针对三种类型的地标执行不同的策略。对于圆柱体，直接移除这个地标。对于平面和直线，将圆柱体拟合到这些点。如果新产生的RMSE较小，使用圆柱体对这些点进行建模。否则如果它是线的话就删除它，因为移除平面可能会导致在某些非结构化环境中对位姿的约束不足。

6.PLCR

a.扫描内运动

采用线性插值来补偿扫描内运动

b.约束

相对位姿内的旋转幅度相对较小，所以采用一阶泰勒展开来近似旋转：

来自平面的约束

：

来自线的约束

:

来自圆柱体的约束

c.相对位姿估计

定义

和

，那么PLCR的代价函数具备如下的形式：

7.实验

总的来说，在室内室外环境都达到了sota性能，并且达到了实时性。

a.KITTI

b.室内数据集

c.时间性能

Conclusion

这篇文章介绍了一种新的使用平面、线和圆柱体的LiDAR SLAM，并且作者证明了通过一些预处理，局部和全局 PLCA 的最小迭代次数与从平面、线和圆柱体捕获的点数无关。此外还提出了 PLCR 问题的有效解决方案。最终的实验结果表明，所提出的算法优于最先进的方法并达到了实时性能。文章的文字主体部分是定理引理公式的引入及推导，建议有兴趣的读者可以附录进行深入了解。

本文仅做学术分享，如有侵权，请联系删文。