【论文笔记】高效、稀疏的三维激光SLAM ： A Novel 3D LiDAR SLAM Based on Directed Geometry Point and Sparse Frame

中国科学院自动化研究所

基于scan2map的三维激光slam鲁棒性好, 深度信息精确, 是相关领域的研究热点。但是，为了达到满意的定位精度，还需要构建一个大的稠密地图，这导致scan2map的匹配非常耗时。

为了解决这个问题，本文提出了一种基于有向几何点（DGP）和稀疏帧的新型3D激光SLAM（包括闭环检测以及位姿图优化）。前者用于在空间维度上提供点的稀疏分布，后者用于在时间序列中产生帧的稀疏分布。该策略提升了三维激光SLAM的效率和里程计位姿估计的精度。为了补偿稀疏帧导致的定位和建图的精度损失，本文使用point propagation方法来提升有向点的质量和scan2map的匹配精度。

---

论文方法 DGP-SLAM

有向点的表示：（x,y,z,i,j,k）, 坐标 + 单位方向向量
从激光雷达的一帧中提取：

• directed edge point set $L_t$, 点云中的点的单位方向向量代表所在直线的方向
• directed planar point set $P_t$，点云的单位方向向量代表点所在平面的法向量。

问题描述:
给定帧$C_t$（t=0,1,2，…）中的三维激光雷达点云，提取有向边点和有 向平面点集$L_t$和$P_t$，估计位姿变换$T_{k_{\alpha }{C}_t}$，并通过scan2map来保留fusion frames的有向点集$L_x^y$和$P_x^y$。在此基础上，利用关键帧上的有向点集进行闭环检测，并在关键帧之间执行全局位姿图优化。

处理流程概述

DGP-SLAM 包括DGP extraction, scan-to-map matching 以及loop closure 三个部分。首先对新的一帧进行有向点提取，然后根据有向点在局部图中通过迭代数据关联和位姿优化进行scan2map匹配。接着依次执行帧选取和点传播来更新局部地图。最后闭环检测模块结合关键帧的有向点集以及帧间相对位姿来检测闭环以及优化全局位姿图。
算法输出该新的帧的全局姿态和一个遍历区域的全局地图。

一、DGP Extraction

有向点是从原始点云数据中生成的新的点，其坐标是点云邻域的中心，方向向量是所在边的朝向或所在面的法向量。

首先将地面点云分离出来作为平面点，

地面点提取方法：《fast segmentation of 3Dpoint clouds: A paradigm on LiDAR data for autonomous vehicle appli-cations》

然后在非地面点云中提取edge points和planar points，提取方法与loam相同

在此基础上，对每个未遍历的边或平面点，通过KD树的半径搜索得到相邻点u1，u2，···um，并计算领域的中心$c_u$和方向向量${\hat{d}}_u$，其中方向向量对应于矩阵${\sum }_U=\frac{1}{m}{\sum }_{i=1}^m(u_i-c_u)(u_i-c_u)$的最小或最大特征值的单位特征向量，取决于是平面点还是边缘点。
然后将（$c_u$,${\hat{d}}_u$）添加到Lt或者Pt中，并将u1，u2，···um标记为遍历。

注意这里的有向几何点的数量远小于几何点的数量，因为需要多个几何点才能生成一个有向几何点。

二、Scan-to-Map Matching

这一步是DGP_slam的核心所在。
1. Frame Selection
定义了三种帧：fusion frame, key frame, non-fusion frame
该步骤的目的就是决定当前帧属于哪一种帧。

假设当前帧 $c_t$ 之前的最近的fusion frame是 $K_{\alpha }^{\beta }$,如果二者之间的位姿距离$\Vert t_{W{K}_{\alpha }^{\beta }}-t_{W{C}_t}\Vert$大于阈值，则将$c_t$ 标记为fusion frame $K_{\alpha }^{\beta +1}$。否则将其标记为新的关键帧$K_{\alpha +1}$

2. Pose Estimation
该模块通过基于有向点的迭代数据关联和姿态优化 ，求解当前帧$c_t$到最近的关键帧$K_{\alpha }$之间的相对位姿$T_{K_{\alpha }{C}_t}$。

1. 首先，将最近的N个fusion frames中的有向点集投影到与当前帧最近的关键帧$K_{\alpha }$中，从而构建局部地图。

2. 其次,迭代地进行数据关联和位姿优化（优化的位姿是关键帧到当前帧的相对位姿），每一步的初始化值为：

   • $T_{K_{\alpha }{C}_0}=T_{K_{\alpha }{C}_1}=I$
   • $T_{K_{\alpha }{C}_t}=T_{K_{\alpha }{C}_{t-1}}{\left(T_{K_{\alpha }{C}_{t-2}}\right)}^{-1}{T}_{K_{\alpha }{C}_{t-1}},t\ge 2$

3. 初始化完成后，当前帧$C_t$中的有向点集与局部图之间的数据进行关联：
   $C_t$中的有向边点和有向平面点根据位姿变换$T_{K_{\alpha }{C}_t}$投影到关键帧$K_{\alpha }$中，然后通过以下三个严格标准来确定局部地图中的候选关联点：

   1. point-point distance。该定向几何点的投影位置与候选点的投影位置之间的距离小于r1
   2. point-line or point-plane distance。有向几何点投影到候选点所在的直线或平面的距离小于D1
   3. direction difference。投影的有向几何点的方向与候选点方向之间的夹角小于θ1

具有最小点直线或点面距离的候选点被认为是最佳关联.

假设对于当前帧$c_t$中的有向边点$l_i^t$和有向面点$p_i^t$，其对应的最佳关联分别为fusion frame $K_a^b$中的有向边点$l_j^{a,b}$,和fusion frame $K_c^d$中的有向面点$p_k^{c,d}$，则需要将这两个最佳关联数据从fusion frame投影到关键帧$K_{\alpha }$中：

都投影在了关键帧中，就可以计算误差然后对相对位姿进行优化了：

将优化的目标函数转换到欧氏空间得到：

使用LM算法进行优化。
重复上述数据关联和姿势优化的迭代过程NI（3）次，以获得最终的姿势估计结果
3. Point Propagation
根据上一步获得的相对位姿，重新进行数据关联，只不过点线、点面、朝向距离的阈值更小了（更严格），保证关联的数据对一定是正确的。

• 如果$c_t$被识别为fusion frame, 则将$c_t$中的关联数据点的有向点加入到其有向点集合中，然后执行前向传播，即 用 之前的fusion frame中的有向点 更新 $c_t$中的、关联到的对应有向点，然后将之前的fusion frame中的有向点从之前的fusion frame中删除。
• 否则， 执行反向传播：即 用 $c_t$中的、关联到的有向点 更新 之前的fusion frame中的对应有向点。

那么具体的，如何进行有向点的更新呢？
论文提出了增量更新的机制，即对点集U 和点集V 的和矩阵（sum matrixs）、和向量（sum vectors）进行更新：
${\Phi }_U={\sum }_{i=1}^m{u}_i{u}_i^T,{\tau }_U={\sum }_{i=1}^m{u}_i$
${\Phi }_V={\sum }_{i=1}^n{v}_i{v}_i^T,{\tau }_V={\sum }_{i=1}^n{v}_i$
经过变换矩阵T=[R,t]的变换后：
$$\begin{array}{rl}{\Phi }_V^{\prime }& =R{\Phi }_V{R}^{\top }+R{\tau }_V{t}^{\top }+t{\tau }_V^{\top }{R}^{\top }+t{t}^{\top }\\ {\tau }_V^{\prime }& =R{\tau }_V+t\end{array}$$
根据变换后的和矩阵、和向量对原来的和矩阵、和向量进行更新：

${\Phi }_U\leftarrow {\Phi }_U+{\Phi }_V^{\prime }{\tau }_U\leftarrow {\tau }_U+{\tau }_V^{\prime }m\leftarrow m+n$
根据更新后的和矩阵以及和向量，计算出提取DGP时用到的矩阵:
$${\Sigma }_{U\cup V}=\frac{1}{m}{\Phi }_U-\frac{1}{m^2}{\tau }_U{\tau }_U^{\top }$$
有了这个矩阵，重新执行一次DGP Extraction得到更新后的点

三、Loop Closure

该步骤在关键帧上执行闭环检测以及位姿图优化，以分开的线程执行。
一旦当前帧被识别为新的关键帧，则获取最近的关键帧$K_{\alpha }$的有向点集，执行闭环检测：

1. 首先，在关键帧$K_0$ 到 $K_{\alpha -S}$之间，选择与$K_{\alpha }$空间上最接近的闭环候关键选帧。S是为了避免闭环候选帧与$K_{\alpha }$太过接近。
2. 若选择到的闭环候选关键帧为%K_i%，则分别构建$K_{\alpha }$、$K_i$附近的局部地图，然后执行scan matching。这里scan matching 使用的是ICP，而不是之前的数据关联，因为两个局部地图可能相距较远。
3. 若scan matching的得分大于阈值，则视为检测到了回环。则将$K_{\alpha }$、$K_i$之间的相对位姿加入到位姿图中作为闭环系数，同时当前帧与$K_{\alpha }$之间的相对位姿$T_{K_{\alpha }{K}_{\alpha +1}}$也被加入到位姿图作为里程计系数。
   使用论文《ISAM2: Incremental smoothing and mapping using the bayes tree》的方法进行位姿图优化，得到新的位姿：${\hat{T}}_{K_x{K}_{x+1}}(0\le x\le \alpha )$，以及包含了有向边点和有向面点的全局一致的有向几何地图。

实验结果

消失实验

kitti dataset

Newer College dataset

速度对比

地图规模对比

可以看出，由于利用DGP特征融合帧的稀疏性，本方法的地图尺寸明显小于其他方法，也为高效的scan2map匹配奠定了基础。