【论文笔记】实时、SOTA的激光里程计 Efficient LiDAR Odometry for Autonomous Driving 2021

浙江大学

地面点在BEV中处理，非地面点在range-image中处理，解决了range-image中的相邻的地面像素点在实际中相距缺非常远从而增大了匹配误差的问题。采用scan2model的方式提升匹配精度并达到了SOTA，但是局部model使用三张二维image来存储，因此内存消耗很小且有界。最终在嵌入式设备上的速度达到了37fps, 可以说是精度、速度、内存消耗的集大成者。

概述：

现有激光里程计方法：

1. 基于搜索树的方法：处理大场景的点云不够高效
2. 基于柱面投影的方法：处理与地面接近平行的激光线束会遇到问题。

本文基于以上问题，提出了

1. 一种基于非地面点的柱面投影方法，并利用鸟瞰图来处理地面点。
2. 引入了range自适应的方法来鲁棒地估计局部表面法向量
3. 提出了一种快速且记忆有效的模型更新方案，以在不同的时间戳上融合点及其对应的法向量。
4. 速度达到了169fps with promising result

论文方法：

将地面点和非地面点通过分割，分开进行处理：

• 地面点：在鸟瞰图中操作，构建ground cost E_G
• 非地面点：利用提出的range 自适应法向量估计方法计算法向量，通过ICP对齐.构建non-ground cost E_S

1. Efficient LiDAR Odometry for Autonomous Vehicles

frame-to-mode的方法。首先将当前帧的三维点云投影成range map，然后基于range-map 进行分割得到地面点和非地面点，地面点被投影到鸟瞰图中计算E_G。对range-map的法向量进行估计，用于ICP配准来计算相对位姿。最后，对局部地图进行更新。

激光点云中有一半是地面点，尤其在高速公路场景中，有益于匹配的面特征基本都是来自于地面点，非地面点 提供的有效特征点较少，这种情况下地面点反而有利于匹配。因此作者认为地面点对于特征点匹配是非常有价值的、是不应该被抛弃的。

那么地面点应该如何处理呢？

对于地面点和非地面点的特征分析：

range-image中的相邻的地面点在3D空间中的实际分布非常稀疏，也就是说range-image中的相邻的地面像素点，在实际中相距是非常远的，不属于同一 local surface。

上图中F123是non-ground points, G123是ground-points，他们在range image 和 鸟瞰图中的位置如下图:

可以看到，在range-image以及 BEV 中，相邻像素中的点在3D空间中的距离是不一样的。

• 对于non-ground points ，F123属于同一 local surface，因此在range-image中被投影到了相邻像素。

• 对于ground-points, 由于激光线束与地面接近平行，G123之间的实际距离较远但是在range-image中仍然被投影到了相邻像素，这意味着位于 G1G2 之间的点在range-image上要么会被投影到G1要么被投影到G2，这样在匹配时会导致很大的误差。而在BEV中，G123三点之间的相对位置关系则非常明确。

因此作者选择BEV来处理地面点。

然而，在BEV中F123被投影到了同一个点，因此作者提出一种融合方法将non-ground points 的range-image 和 ground points的BEV 的 代价函数 E_S、E_G 同时考虑进来：
$$\underset{T_t^{t-1}}{\min }w{E}_S+(1-w)E_G$$
使用匀加速运动模型给初始化相对位姿。

使用高斯牛顿优化算法来求解上述优化问题。在每个迭代中位姿更新量的计算为
$$\Delta T={\left(J^{T}J\right)}^{-1}{J}^T\mathbf{e}$$
其中e为残差向量，雅各比矩阵J的计算：$J=w{J}_S+(1-w)J_G$

最后根据迭代量求出相对位姿变换。

2. Non-ground Cost E_S（优化scan与model之间的重投影误差）

用range-image进行计算，尺寸2048 * 80。首先将range-image表示为三维的vertex map V_D。

计算cost E_S:

考虑到消除漂移，论文使用了scan-to-model的方法做匹配，这里局部model使用三维的vertex map V_M以及法向地图N_M来表示。

通过最小化点面距离求解相对位姿：
$$E_S\left({\mathcal{V}}_D,{\mathcal{V}}_M,{\mathcal{N}}_M\right)=\sum _{\mathbf{u}\in {\mathcal{V}}_D}{\left[{\mathbf{n}}_u^T\left(T_t^{t-1}\mathbf{u}-{\mathbf{v}}_u\right)\right]}^2$$
这里$\mathbf{u}\in {\mathcal{V}}_D$， ${\mathbf{v}}_u\in {\mathcal{V}}_M$， ${\mathbf{n}}_u\in {\mathcal{N}}_M$，分别表示scan中的vertex与model中对应的vertex点和normal点。对应关系是通过在range-image中确定的：
$$\begin{array}{rl}{\mathbf{v}}_u& ={\mathcal{V}}_M\left({\Pi }_S\left(T_t^{t-1}\mathbf{u}\right)\right)\\ {\mathbf{n}}_u& ={\mathcal{N}}_M\left({\Pi }_S\left(T_t^{t-1}\mathbf{u}\right)\right)\end{array}$$
这里${\Pi }_S$是柱面投影函数。 最后就可以求解出雅各比矩阵：
$$J_S={\mathbf{n}}_u^T\left[I{\left[{\mathbf{v}}_u\right]}_{\times }\right]$$
$[{\mathbf{v}}_u{]}_{\times }$表示${\mathbf{v}}_u$的斜对称矩阵。

SVD 计算法向量

这里提出了一种范围自适应的法向量估计方法.

首先通过SVD计算局部点集中的协方差矩阵：
$$\Sigma =\frac{1}{k}\sum _{i=1}^k\left({\mathbf{p}}_i-\overline{\mathbf{p}}\right){\left({\mathbf{p}}_i-\overline{\mathbf{p}}\right)}^T,\overline{\mathbf{p}}=\frac{1}{k}\sum _{i=1}^k{\mathbf{p}}_i$$
特征分解后得到三个特征值，选择最小特征值（表示点集在该特征向量方向的方差）对应的特征向量作为法向量。

此外使用三个特征值来定义点的曲率用于提取显著性平面点：
$${\sigma }_{{\mathbf{p}}_i}=\frac{{\lambda }_3}{{\lambda }_1+{\lambda }_2+{\lambda }_3}$$
在计算时需要定义邻域范围大小进行计算，而户外的激光点云变化范围大，固定的邻域尺寸并不够高效和灵活。作者提出基于点云距离ｒ，图像尺寸（ｗ＿ｓ，ｈ＿ｓ）以及预定义的搜索阈值$\delta =0.3$来计算邻域搜索尺寸：
$$\left(\begin{array}{l}{l}_x\\ l_y\end{array}\right)=\left(\begin{array}{c}\max \left(\min \left(\frac{\delta }{r\pi }{w}_S,l_x^{\max }\right),l_x^{\min }\right)\\ \max \left(\min \left(\frac{\delta }{rf}{h}_S,l_y^{\max }\right),l_y^{\min }\right)\end{array}\right)$$
最后的搜索范围用最小值（5，3）和最大值（13，7）进行裁剪。

去除外点

由于边界的不连续性和多重反射性，协方差矩阵的分解对异常值非常敏感。需要去除离群点：

• 如果一半的点到参考点的距离大于阈值0.5m，则认为该点是离群值
• 当点到平面的距离$d_{p2p}=(q-p{)}^T\ast n_p$大于阈值0.5时，该点被标记为离群值

3. Ground Cost E_G

地面点分割

首先基于预定义的z轴方向的阈值对非地面点进行过滤。

其次对于两个相邻的地面点，竖直方向上的角度距离应该小于5°：

对于在range -image中的点P，在其上下邻域中计算其与领域点的角度距离：
$${\theta }_{up}=\arctan \frac{{\lfloor {\mathbf{p}}_{up}-\mathbf{p}\rfloor }_z}{{\lfloor {\mathbf{p}}_{up}-\mathbf{p}\rfloor }_{xy}},{\theta }_{\text{down }}=\arctan \frac{{\lfloor {\mathbf{p}}_{\text{down }}-\mathbf{p}\rfloor }_z}{{\lfloor {\mathbf{p}}_{\text{down }}-\mathbf{p}\rfloor }_{xy}}$$
当两个角度距离均小于5°的阈值时认为是地面点。

鸟瞰图投影：

尺寸120m * 60m.分辨率0.1 * 0.1

利用BEV将scan中的地面点B_G与model中的BEV map B_M进行配准：
$$E_G\left({\mathcal{B}}_G,{\mathcal{B}}_M\right)=\sum _{\mathbf{g}\in {\mathcal{B}}_G}{\left[{\mathbf{n}}_g^T\left(T_t^{t-1}\mathbf{g}-{\mathbf{v}}_g\right)\right]}^2,{\mathbf{v}}_g\in {\mathcal{B}}_M$$
同样的，vertex之间的匹配关系是通过二维BEV -image 得到的：${\mathbf{v}}_g={\mathcal{B}}_M\left({\Pi }_G\left(T_t^{t-1}\mathbf{g}\right)\right)$，其中${\Pi }_G$是俯视投影函数。

最后可以求解出雅各比矩阵：
$$J_G={\mathbf{n}}_g^T\left[I{\left[{\mathbf{v}}_g\right]}_{\times }\right]$$
注意在这里法向量的计算与之前预先计算好的不同，是在线计算的（因为对于地面点来说，range-image中相邻的点相距太远，不能很好的反映局部的曲率）

实时计算法向量：

将scan中的地面点g 投影到local map v_g中，根据邻域内最近的5个点计算表面法向量。由于有2D 的BEV map最近邻搜索可以很快的完成（on GPU）

局部地图更新机制

提出了一种非常快速和高效的模型更新方案，该方案利用了二维range-image和地面点的BEV map，且可以很容易地并行实现。

时间窗10s，对range-image 和BEV map分别使用不同的更新策略：

• 对于非地面点：维护一个vertex map和一个normal map

  对于前后帧的两个对应点，保留距离激光雷达原点更近的那个点

• 对于地面点：维护一个BEV vertex map B_M，其法向量是在ICP匹配过程中根据最近邻的5个点动态计算的

  更新机制上同。

因此，总的地图只需要维护三个2D image即可，存储上非常高效。

实验

主机： i7 9750h + RTX 2060 169 fps

嵌入式平台： NVIDIA Jetson AGX 37fps

精度SOTA：