Loam livox论文学习笔记
Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV
参考：多激光雷达的协同定位建图及在线外参自标定
官方自动标定程序
LeGO-LOAM和LOAM的区别与联系
LOAM-SLAM原理深度解析
首先感谢香港大学林博等人的慷慨，将源码和论文提供给大家学习，本文的主要内容也是根据其实验室发表相关论文以及其他平台资料整理得到。
LOAM算法
论文
算法
核心包含特征提取(Lidar Registration)和里程计结算(Odometry and Mapping)
通过高频率的里程计(Odometry)实现粗定位，通过低频率的里程计(Mapping)实现精确定位。
为了提高算法效率，通过计算点前后5个点的曲率，使用平面和边缘作为点的特征标准。
然后做帧与帧之间的特征匹配，首先需要做运动补偿，然后通过点线和点面距离作为优化参数，求最小值。这里结果只是局部的位姿变换，为了精度还需要帧与全局地图（map-map）的位姿关系。
map-map的方法对象是最近10帧地图和全局地图做匹配，为了计算的效率，使用10m方形的局部区域代替全局。
优点：
- 新颖的特征提取方法（边缘平面）
- 运动补偿（时间戳）
- 融合局部和全局思想
缺点：
- 没有后端优化
- 不能处理大规模旋转（旋转向量求解）
LeGo-LOAM和LOAM的区别和联系：
LeGo-LOAM是以LOAM为框架而衍生出来的新框架。前者更改特征点的提取形式，添加后端优化。前者提升主要在于轻量级和地面优化。
算法核心包含：分割、特征提取、雷达里程计、雷达建图。
Loam livox论文的主要贡献：
- 对Livox激光雷达视野小提出解决方案：
- 使用点到线和点到面的icp方式。
- 做了运动补偿：使用分段并行化处理的方式，相比插值效果和效率上效果更好。
- 消除运动物体和异常值影响：每次去除误差最大的20%的点。
- 消除一部分点，例如边缘，强度过大或过小的点，在物体后面的点，提高精度。
- 不需要其他传感器的参与。
- 加入反射强度作为第四维标准，用来识别不用材料形成的边缘（墙、门）。
一些问题和解答
后期map数量级高的时候，会不会降低整体运行速度？
回答：源代码的投影分为两种方式，一种是全局建图然后和当前帧做匹配，这样精度较高（隐式回环）但是速度会降低；另一种方式是和短期内最新一段时间累积的地图去做匹配，速度快，但是精度相对较差。这两种方式在开源项目中有选项可以选择。
看视频效果很惊艳，但感觉只能用在运动比较平缓的场景？
回答：对于motion其实还行吧，只要旋转的角速度不要太快，一般都能hold住；我倒觉得主要是和feature有关，只看到天空或者面对一堵大墙结果就飘了。
多传感器优势：
- 多传感器方案比单个传感器方案便宜
- 提高点云覆盖率
- 减少单个传感器的遮挡问题
- 增强鲁棒性
- 感知多元化：不同测量距离的传感器融合
去中心的分布系统优势和劣势：
- 基于性能考虑：不同传感器使用不同设备运算，实现并行计算，节约时间的同时避免设备负荷太大或者要求太高。
- 基于鲁棒性：减少系统崩溃的可能。
- 标定问题，抗震问题，刚性连接。
- 融合算法要求，数据的判定标准。
GPS和RTK
GPS是利用差分原理，通过基站测量消除误差，将误差数据反馈给客户机，修正数据。此方法精度可以达到亚米级。
RTK叫载波相位差分技术，通过基站发送的带有相位的载波来进一步减小误差。
论文正文
代码
摘要
LiDAR odometry and mapping(LOAM)可以同时确定机器人位置并且建立周围环境的高精度、高分辨率的地图。这有利于车辆的自动导航和安全的路径规划。这篇论文展示了一种应用于小视野和不规则采样激光雷达的鲁棒实时LOAM算法。我们通过前端和后端的改进，解决这一款激光雷达带来的问题，并且在精度和效率上和现有方法比更加优秀。为了分享成果，代码放在github上。
简介
固态激光雷达有多种实现方式：微机电扫描系统(micro-electromechanical-system, MEMS)，光学相阵(optical phase array, OPA)，里斯利棱镜(Risley prism)。
固态激光雷达有价格便宜，可靠性高，部分性能优于传统激光雷达的特点；同时也有一些新的特性（以Livox激光雷达为例），例如小视野、不规则扫描模式、不重复扫描、运动模糊会影响激光雷达的实际使用。
- 小视野：因为系统中镜面旋转角度有限，视野有限。会导致一帧中包含特征少，使得后续的特征匹配容易退化，易受运动物体干扰。
- 不规则扫描方式：玫瑰线扫描方式给特征提取带来困难，因为相邻扫描花瓣可能相距很远。所以这里直接去掉原来LOAM的帧到帧匹配，直接采用帧到地图的匹配。
- 非重复扫描：为了增加试场覆盖率。
- 运行模糊：帧内运动导致点云扭曲和运动模糊。
为了解决上述问题，我们开发了一个软件包“Loam_Livox”，可以解决特征提取，运动物体过滤和运动失真补偿。并且不需要其他例如IMU,GPS和相机。本文算法通过将点云配准到指定的局部地图范围内计算激光雷达的位姿。
相关工作
icp算法的提出和改进：
Besl(“Method for registration of 3-d shapes,”)首次提出了ICP算法，Mendes(“Icp-based pose-graph slam,”)在此基础上提出一种位姿(pose-graph)SLAM来纠正在一系列帧匹配中产生的漂移。ICP算法要求传感器拍摄同一场景的同一物体。为了解决这个问题，Pulli(10)提出一种点到面的误差度量标准，该方式和点对点的标准一同使用在(８)中，并且被叫做广义ICP(11)。Zhang(12)和Shan(13)也在激光雷达里程计和建图中使用采用点到边的标准。
除了基于几何特征的方法，还有基于3D特征点的方法(14-16)。这些方法通过一些检测器，例如Point Feature Histograms (PFH,14,15),Viewpoint Feature Histograms (VFH,16)等。
考虑到传感器特性和实时性需求，我们选择点到边和点到平面距离(12,13)。
运动补偿：
之前学者通过线性插值在前端弥补运动带来的影响，还有人(Gentil,19)通过后端优化过程处理运动补偿问题。后端算法可以取得很好效果，但是不能实时运行。
因为前人方法基本基于旋转激光雷达，这篇论文在固态雷达上做出一些贡献：
(1). 做出一套基于小视野固态雷达的LOAM算法。
(2). 通过在前端处理中考虑激光雷达物理特性增加LOAM算法效果精度和鲁棒性。
(3). 我们提出一种简单且高效的运动补偿方式－分段处理且并行化处理。这种方式比线性插值法在精度和运行效率上都有提高。
点选取和特征提取
系统的概览图如下图所示
前端处理包括点选取和特征提取。由于激光雷达的特性（点云光斑大小、性噪比等），我们通过点的等级选取“优点”。
点的选取：
我们假设x,y,z分别对应前，左，上。我们按照如下公式计算特征：
为了增加建图和定位精度，我们去除满足以下条件的点：
- 靠近边缘的点（夹角大于等于17°）
- 强度过大或者过小的点（小于0.007或者大于0.1）。强度过大可能导致接受电路饱和失真，太小导致低性噪比，都会降低精度。
- 入射角(incident angle)接近π或者0（小于5°，大于175°），因为这样角度入射的激光点会被拉长。
- 藏在物体后面的点（如图示e点），这可能造成边缘特征的误识别。这样点满足以下式子：
（此标准如何得到）
特征提取：
通过候选点的局部光滑特性来提取平面特征和边缘特征。为了解决小视野和点的选择带来的特征点数量减少，缓解由此带来的匹配退化的问题，我们将反射强度加入形成四维测量标准。这样可以检测出材料不同产生的边缘（比如墙和门）。
迭代位姿优化
由于固态激光雷达非重复扫描的特性，我们采用一种迭代位姿优化方法来计算激光雷达的位姿。更新频率20Hz。
边缘到边缘的残差：
εk和εm分别代表当前帧和地图中所有边缘特征。为当前帧每个点在地图中找5个离的最近的点，如下图所示。
通过KD-tree可以加速搜索速度，KD-tree通过另外一个线程实时建立。
我们通过重投影的方式找到最近点，公式如下：
(Rk; Tk)是激光雷达当前帧最后一个点扫描结束后的位置，也是我们的优化目标。
为了确定地图上找的５个点在同一直线上，计算µ和协方差矩阵，如果矩阵特征值最大值是第二大值三倍以上，则在同一直线上。
然后误差计算公式：
面到面的残差：
我们也是在地图平面上找五个最近邻点计算协方差矩阵，如果矩阵特征值最大值是第二大值三倍以上，则在同一直线上，残差公式如下，作为优化标准：
帧内运行补偿：
我们通过以下方式做补偿：
- 分段处理：我们把输入的帧分为三个连续的子帧。然后这三帧分别和实时更新的地图做匹配。都以子帧最后一点的位置作为激光雷达这一帧的位置。这种方式效果很好，而且可以通过多线程加速过程。
- 线性插值：前一帧和当前帧的变换矩阵如下：
然后我们记tk−1为上一帧最后一个点时间， t为当前帧最后一个点的时间，按照如下的式子做插值：
s = (t−tk−1)=(tk−tk−1)
θ是旋转的大小， ω \omega ω是 R k − 1 k R_{k-1}^k Rk−1k旋转向量 ω ^ \hat{\omega} ω^是 ω \omega ω斜对称矩阵。由罗德里格斯公式可得：
上式子可以节约计算时间，然后计算当前帧雷达的位置变换矩阵：
然后使用重投影误差的方式计算代价函数如下：
异常值过滤，动态物体过滤
在位置优化的每一轮迭代中，我们重新找每个特征点的最近领点，并且在目标函数加入边缘残差和平面距离残差。首先迭代几次，用结果计算残差，剔除最大的20％点云，然后做完全的位置优化。过程如下：
结果
建图评价：
两种运动补偿方法效果如下图所示：
可以看出没有补偿的在台阶栏杆处模糊，并且在大范围内产生畸变。有补偿效果较好，但是线性插值在长时间积累下会产生可观的漂移，因为手持设备运动不稳定，差值当时不能准确描述结果。
评价里程计：
我们将我们算法得到定位信息和GPS得到的信息进行比较，如下图所示：
我们比较两个位置距离在两种方法下得到的数据，说明位置精度很高。
我们也通过运动捕捉设备比较了角度精度，结果如下：
结果显示两种方式得到的路径基本一致，平均差值在1.1°。
评价运行性能：
通过使用多线程、子帧和KD-tree搜索特征点对，速度可以快两三倍。
结论和讨论
这篇论文的方法继承了传统的LOAM算法的内容（特征提取、匹配、线性动态补偿），但是也有一些重要的贡献，例如点的选取，位姿优化的迭代和多线程的使用。该方法可以在20Hz的频率下实时运行。虽然建图和定位精度较高，但是顺序扫描本质上会带来漂移。闭环和滑窗优化可以解决漂移问题，这个研究会放到后续工作当中。