一文详解固态激光雷达的里程计（loam_livox）
作者：Liam
点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
Loam livox(2019 IROS)
介绍： 大疆出品，必属精品。 固态激光雷达里程计的工作现阶段还是比较少的，大疆自己出了固态激光雷达后在LOAM的基础上改进了一个适用于固态激光雷达的里程计，该系统可以用在小视场角和非重复性扫描的雷达上。
代码地址：在公众号「3D视觉工坊」，后台回复「loam_livox」，即可直接下载。
需要知道的介绍
需要知道的是（文章最后有相关的技术手册）:
- 固态激光雷达的视角很小，Livox MID40的视角仅有38.4度。但是想要获得大的视角可以使用多个固态的激光雷达（比如MID-100其实就是3个MID-40）。
- 扫描图案不均匀，原来的激光雷达规则扫描，极大的简化了特征提取和匹配。但是固态激光雷达扫描不均匀Livox MID40扫描的图案类似花瓣，相邻的两次扫描距离很远。
- 非重复扫描，这样可以最大化覆盖率（尽可能多的得到场景中的信息）。
- 运动畸变，同一帧的点云扫描有先后顺序的，由于运动可能导致同一帧的点云出现运动畸变。
为了解决上述的问题，作者创建了Loam_Livox工具包。解决了在小的视角下进行特征选择和提取的任务，有严格的外点剔除策略，移动的对象滤波和运动畸变补偿。在没有诸如IMU、相机、GPS等传感器的基础上，利用点云和局部地图的匹配鲁棒的实时计算雷达的位姿。
相关工作
目前有点到点,点到面和点到边缘的匹配方式，3D的特征有Point Feature Histograms (PFH)和viewpoint Feature Histograms (VFH)等。本文采用的是点到平面和点到边缘的匹配方式，为了解决运动畸变的问题，可以在前端进行线性插值，最近有的方法提出也可以在后端利用优化的方法进行畸变矫正。虽然优化的方法更好但是不实时运行。 本文的主要贡献有:
- 针对小视角的雷达提出了完整的LOAM算法
- 通过考虑low-level的雷达参数增加了LOAM算法的精度和鲁棒性
- 提出了一种简单但是有效的运动补偿的方法。分段处理，并使其实现并行化。
特征选择和提取
系统概述如下图所示，其前端处理包括点的选取和特征的提取。考虑到激光雷达传感器的low-level物理特性(如激光光斑大小、信噪比)的测量机制，我们进行点级选择来提取“优点”。
A.点的选择
- 计算点距离雷达的距离D(P)=x2+y2+z2
- 计算光束和X轴的夹角ϕ(P)=tan−1((y2+z2)/x2)
- 计算反射率I(P)=R/D(P)2，其中R是点的反射强度
当反射率I小的时候证明物体离雷达很远，反射强度也很小。
θ=((Pa−pc)×Pb|Pa−pc|×|Pb|)
为了提升定位和建图的精度,我们需要删除以下的这些点:
- 接近FoV边缘的点。 （例如φ（P）≥17◦对于Livox MID40)在这样的区域，扫描轨迹曲率大，导致特征提取不太可靠。
- 强度太大或者太小的点
- 点的入射角大于0度或180度.
- 删除物体后边点
B.特征检测
选择点后，我们执行特征提取以从“好点”中提取特征。我们通过计算候选点的局部平滑度来提取平面和边缘特征，如[1]所示。 此外，为了减轻由于有限的FoV和点选择导致的少量特征而导致的匹配退化，我们将LiDAR反射率用作第4维测量。 如果3D点的反射率与其相邻点有很大不同，我们将其视为边缘特征点（由于材料变化而导致反射率的边缘，与由于形状变化而导致几何形状中的边缘相反）。在某些退化情况下，例如面对带有关闭的门窗的墙壁，这些点是有益的。
迭代的位姿优化
第一节提到的非重复扫描导致提取的特征无法像[12、13、19]中那样不断跟踪。一个简单的例子是，即使LiDAR是静态的，扫描的轨迹（和特征点）也不同于前一帧。 在我们的工作中，我们使用迭代的方法进行姿态优化来计算LiDAR位姿。通过稍后详细介绍的适当实现，我们可以实现20Hz的实时里程计和建图。
A.边和边的残差
其实就是点到线的距离，分子的叉乘计算的是三个点组成的三角形的面积，分母是底的长度。所以计算的就是三角形的高即点到线的距离。
B.面和面的残差
与边缘特征点相似，对于平面中的点当前帧的特征集Pk，我们在地图的平面特征集Pm中找到5个最近的点，同时通过计算它们的协方差矩阵Σ来确保这5的点确实在同一平面内， 如果最小Σ的特征值比第二小的特征值小三倍，我们计算当前帧的地图中平面点到由这5个点组成的平面的距离，然后将此残差添加到姿势优化中。
分子后边的叉乘部分还是地图三个点组成的三角形的面积，再点乘一得到四面体的体积，再除以底面积得到四面体的高即点到平面的距离。
C.图像帧运动补偿
由于激光采集的点云并不是在同一时刻采集的，所以就会存在运动畸变（坐标系不同引起的），所以需要根据接收激光点的时间计算位姿把点云投影到同一坐标系。为了补偿每次扫描的时间和位姿不同，我们可以利用：
分段线性：把一个新来的帧分成三个连续的子帧，然后把这三个帧独立的和现阶段构建的地图做匹配。在每个子图做scan-match的过程中利用子图中最后一个点的位姿把所有的点投影到全局地图中，这样每帧采样的时间仅为原来的三分之一。尽管这种方法很简单，但是效果很好（LEGO—LOAM中把一帧分为6份应该也是分段线性的思想），分段线性对于多核的CPU并行运算也有好处。
线性插值：这部分在张籍博士的LAOM中介绍的比较详细，大家在看的时候可以参考。主要思想是在当前帧最后一个激光点得到的雷达位姿[R_k,T_k]和次新帧的[R_k-1,T_k-1]中间的时刻t，利用恒速模型计算中间时刻点的位姿。具体来说：
很容易知道每个点采集的时间（自己计算或者激光雷达采集的时候该信息是可保存的），已经知道了上一帧最后一个点的采集时间t_k-1和当前帧最后一个点采集的时间是t_k，在这中间的当前帧的点采集的时间是t，令s=(t-t_k-1)/(t_k-t_k-1)，所以t时刻的位姿是:
最后论文给出的建图的效果如下所示：
算法一：雷达位姿优化
（1）计算线和线的误差
（2）计算面和面的误差
（3）利用两个特征迭代的优化位姿
（4）删除残差中最大的20%
（5）在迭代中如果位姿收敛就break
D.外点和运动物体剔除
在实际跑的时候避免不了会有动态的车、人及其他物体。本文的动态物体剔除算法是在每一次迭代优化位姿的过程中重新找到每个特征的最近点把边边残差和面面残差加入到目标函数，第一次只优化两次，然后把残差中最大的20%丢弃来达到剔除外点的目的。
精度
室内和室外的定位精度分别是0.41%和0.65%（直接收尾点和GPS比，这个精度没什么可以参考的），在实际测试的过程中发现固态激光雷达在手持的状态下只要晃动的剧烈一点就会丢失，而且由于该激光雷达的特性（非重复性扫描），这个系统不能在快速运动的系统中使用。但是拿视觉或者激光里程计给他提供位姿，在低速的场景中单纯拿他建图效果还是很棒的。最后跑官方数据集的效果如下所示：
实验室的师弟也在实际场景中做了测试，建图效果如下所示：
最后给上调研的时候查到的一些资料： 固态激光雷达的优势： ①其结构简单，尺寸小，无需旋转部件，在结构和尺寸上可以大大压缩，提高使用寿命并使其成本降低。 ②扫描精度高，光学相控阵的扫描精度取决于控制电信号的精度，可以达到千分之一度量级以上。 ③可控性好，在允许的角度范围内可以做到任意指向，可以在重点区域进行高密度的扫描。 ④扫描速度快，光学相控阵的扫描速度取决于所用材料的电子学特性，一般都可以达到MHz量级。 固态激光雷达劣势： ①扫描角有限，固态意味着激光雷达不能进行360度旋转，只能探测前方。因此要实现全方位扫描，需在不同方向布置多个（至少前后两个）固态激光雷达 ②旁瓣问题，光栅衍射除了中央明纹外还会形成其他明纹，这一问题会让激光在最大功率方向以外形成旁瓣，分散激光的能量。 ③加工难度高，光学相控阵要求阵列单元尺寸必须不大于半个波长，一般目前激光雷达的工作波长均在1微米左右，故阵列单元的尺寸必须不大于500nm。而且阵列密度越高，能量也越集中，这都提高了对加工精度的要求，需要一定的技术突破。 ④接收面大、信噪比差：传统机械雷达只需要很小的接收窗口，但固态激光雷达却需要一整个接收面，因此会引入较多的环境光噪声，增加了扫描解析的难度。
MID-40简单使用说明
使用安装
1.将Livox Mid-40连接到Livox转换器。2.使用以太网电缆将PC和Livox Converter连接到路由器。3.将Livox转换器连接到外部电源。 额定功率是10w,电压范围10-16v.需要注意的是温度越低需要的功率就越大,所以需要合适的电源.
点云扫描方式
非重复扫,高覆盖率
MID-40
MID-100
参考文献
[1] Zhang J , Singh S . LOAM: Lidar Odometry and Mapping in Real-time[C]// Robotics: Science and Systems Conference. 2014. [2] Lin J , Zhang F . Loam_livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV[J]. 2019.
本文仅做学术分享，如有侵权，请联系删文。
下载1
在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2
在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。
下载3
在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。
重磅！3DCVer-学术论文写作投稿 交流群已成立扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群。一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。▲长按加微信群或投稿▲长按关注公众号
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款 圈里有高质量教程资料、可答疑解惑、助你高效解决问题觉得有用，麻烦给个赞和在看~
