面向自动驾驶车辆的高效激光里程计（ICRA2021）

柔情只为你懂 2022-08-28 01:52 179阅读 0赞

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

![28c96400f7409f044b780c56091d953d.png][]

作者丨泡泡机器人

来源丨泡泡机器人SLAM

Efficient LiDAR Odometry for Autonomous Driving

下载链接：https://arxiv.org/pdf/2104.10879.pdf

团队：浙江大学

作者：Xin Zheng, Jianke Zhu

学会：ICRA 2021

编辑：周朋伟

审核：张海晗

摘要

LiDAR里程计在自动驾驶导航中占用重要作用，通常将其视为点云的扫描匹配问题。尽管在KITTI里程计数据集中取得了卓越的性能，但是传统的基于KD树的搜索方式仍然难以直接处理大规模的点云数据。SuMa系列的基于深度图像的方法通过球形投影可以快速的完成最近邻搜索。然而，在处理近乎平行的光束的地面点时并不很有效。为了解决这样的问题，非地面点采用球形投影，而地面点采用鸟瞰图的方式进行投影。此外，通过距离自适应的方法来估计局部表面的法向量。我们利用一种快速且内存高效的子图更新方案, 以便融合不同时间戳的点和相应的法线。我们在KITTI数据集上进行了测试证明我们的方案是有效的。

### 背景与贡献 ###

由于工业需求的增加，自动驾驶领域已经吸引了大量的研究工作，其中激光里程计在构建高精度的导航地图方面发挥着重要的作用。高效的激光里程计方案应该对包括城市、高速公路和乡村道路在内的各种环境具有鲁棒性。相比于视觉SLAM的方案，激光SLAM能够利用处理较大的光照变化。通常，激光里程计被认为是局部的点云匹配问题，最常用的方法就是ICP方法，其需要足够的内点数量同时依赖初始值。由于需要利用KD-Tree来完成内点集合的构建，这成为这一类激光里程计方案的瓶颈。近年来，SuMa系列的方案将3D点云转换成2D的球形深度图像，因此匹配点集的搜索就可以高效的在局部的小patch中进行。利用深度图像梯度的叉乘来构造法向量地图，这类方法通常被用于RGBD传感器中。然而点云相比于RGBD更加稀疏，通常在物体垂直于激光线束的位置相对稠密，而在近乎平行于线束的位置相对稀疏。这导致了两个空间临近点在经过深度图像中的距离相对较远。因此，图像的梯度叉乘很难有效的计算法向量。为了解决上述问题, 我们提出了全新的高效激光里程计方案。文章的主要贡献如下：

*  我们提出了一种高效的激光里程计方案, 充分的利用非地面点的球形投影和地面点的鸟瞰图的优势。
 *  一个鲁棒的距离自适应的法向量估计方案被用于点云匹配中。
 *  快速高效的维护深度图像和鸟瞰图的更新方式。
 *  该方案通过KITTI数据集的测试，可以每秒处理超过169帧点云并达到较高精度。

### 方法 ###

#### 1. 系统框架 ####

![c1407f6963770554daeda37d65d51536.png][]

首先原始的3D点云将会被通过球形投影转换成深度图像来帮助完成快速分割和地面点残差。之后，地面点将会从深度图像中分割出来转换成鸟瞰图进而构成地面点的残差的计算。然后，通过距离自适应法向量估计方案完成法向量地图的估计，他们将会被用于ICP的位姿增量的计算。最后我们将会更新非地面点的球形深度子图以及地面点的鸟瞰图。

#### 2. 融合方案 ####

原始的雷达点云大约有一半以上的点是地面点云，尤其是在高速公路上，很难从非地面点集中提取平面特征。因此，非地面点集不足以完成整个位姿估计，而地面点和丰富的平面特征能够很好进行互补。

![d9bf6a9a703fd81b9ad0d23c3a540c7c.png][]

如上图所示，由于地面点相对稀疏，相邻点在经过球形投影转换成深度图像以后，像素的差值相距较大，主要是因为反射平面近乎和激光线束平行所导致的。而非地面点相邻点集在深度图像上的像素时近乎位于同一列的。进一步，由于深度图像是通过角度来对空间完成划分的，这导致相邻激光如或者中间将会含有大量的地面点，这些地面点尤其是远距离的点在图像上的像素差异越大，这将会导致大量的不正确的数据关联。为了能够解决上述的问题，我们将地面点转换成鸟瞰图以便利用额外的地面信息同时完成正确的邻近点集的搜索。如流程图中所示，鸟瞰图是从上到下来捕捉雷达的信息， 但是在鸟瞰图垂直的建筑物表面点将会位于鸟瞰图中相同的像素位置。为了处理这种情况我们提出如下融合方案。

当前帧激光雷达的坐标系为，子图的坐标系为。我们的目标是找到帧到子图之间的相对位姿变换。非地面点的loss和地面点的loss 将会被通过自适应的权重策略构成如下的最小化问题

![fb635a8e08cd4464162b2e246a362da3.png][]

其中 是两个系数的内积。是用户自定义的参数用于平衡地面点和非地面点。表示地面点和非地面点的内点比例。

之后利用高斯牛顿法迭代完成优化，

#### 3. 非地面点残差函数 ####

首先我们将利用球形投影(Spherical Range Image(SRI))转换成深度图像。给定一个空间点，将其从笛卡尔坐标系转换成球形坐标系

![00c799e2351fc6f71b26bdf9468353a8.png][]

其中 表示距离，表示方位角，表示俯仰角。

球形的距离图像将会被当做成的查找表存储笛卡尔坐标在方位角和俯仰角的索引值。最终我们通过如下函数住转换成最终的像素坐标。

![184a729ded5d9db2a8ec93d6d4c72639.png][]

其中是雷达的垂直视场角。and 表示图像的宽度和高度。

#### 4. 非地面点残差函数 ####

非地面点通过球形深度图像可以高效的利用投影数据关联。为了减小里程计漂移，仍是采用 frame-to-model的匹配形式，球形深度图像被记为，它相对应的法向量地图为。之后我们最小化点到平面之间的距离如下

![5f4b2674e8e164e15c2f3f52e57f39fb.png][]

每个顶点通过如下关系投影到子图中的顶点和它的法向量

![e150ea2f4544d47e6073eec507e87412.png][]

非地面点的法向量形式如下

![0312fd998227b6a59d0c951b006ed23c.png][]

#### 5. 基于特征值的法向量估计 ####

上式中的雅克比高度依赖法向量的估计，同时在建立对应关系和计算残差中同样会使用。传统的方式是利用局部图像梯度的叉乘，这在空间点不处于相同的局部平面区域的时候，产生较大误差的估计。因此我们提出一种距离自适应的方式来估计法向量。通常情况下，一个平面模型通常为。给定一个属于局部平面的空间点集，法向量的优化可以同最小化如下误差来进行

![a3b2c26014ff56e4d41592bcc5beca57.png][]

我们可以通过对其协方差矩阵进行特征值分解从而得到闭式解

![75c68ecf05740fd6bde6d5f6f615096d.png][]

最小特征值对应的特征向量即为法向量。

为了能够应对点云的不连续，我们通过距离自适应的临近点搜索方式，每个搜索的patch的尺寸将会根据半径和搜索距离阀值和距离图像的分辨率

![4aede2400e55caac4b46c16c46797ca9.png][]

其中 , and 表示最大和最小的搜索距离

由于特征值分解将会受到异常值的影响，通过以下额外的标准来进一步去除，如果点到平面之间的距离大于给定的阀值。将会被当做是异常值。

#### 6. 地面点残差函数 ####

我们首先使用距离图像的阀值算法来完成地面分割，LiDAR中心到地面点之间的高度为，因此地面点之间的坐标应该一定的高度的范围内and , and 。球形深度图像的列表示相同的方位角空间，如果两个相邻的像素点是地面点，那么这个两个点在高度方向上的角度差值应该小于。我们找到一个空间点在深度图像上的像素坐标是，找到其相邻方位角的像素点and 。在两个方向上的角度的变化可以通过下式来计算

![cf2f43ac156529b8acbc1590a83a6a81.png][]

地面点在两个方向之间的角度阀值应该指定的参数值即and

在获得了地面点之后，类似于球形投影，我们将地面点转换成对应的鸟瞰图中的像素值。

![5f57da5ba21b05ae7bfc87fc9daeb349.png][]

其中and 是鸟瞰图预先设定的范围，and 表示的是两个方向上的分辨率。

鸟瞰图可以用于地面点对应关系的查找表，我们记表示地面点的法向量，整个地面特征的代价函数形式如下

![2527aa0852269e6549694625316b648f.png][]

相应的雅克比矩阵的形式如下

![cea4b1f5bfcb298f2d939307898ed5b7.png][]

其中地面点法向量的计算通过在鸟瞰图中搜索邻近点完成。

#### 7. 地图更新 ####

由于点云的稀疏性，当前帧点云和前一帧的点集通常不会完美的对应。这在一定的程度上增加了里程计的漂移。因此同样选择使用帧到子图的匹配模式。为了较小异常值的影响，我们移除局部子图中时间戳较老的点， 表示的当前点的时间戳，表示局部子图被观测到的点的时间戳。

对于非地面点，我们维护顶点地图和法向量地图。我们通过优化得到的相对位姿将之前的深度图像变换到当前帧中，计算它与当前帧的顶点地图之间的距离。距离激光雷达原点更近的点将会被保存下来，并用于更新局部子图。对于地面点，我们仅维护鸟瞰图的顶点地图，这些点是在ICP计算时用于计算法向量。最终，我们仅仅需要维护2D的地图，这样十分高效。

### 实验 ###

![469f95c350c65fa2e9ee814a1761167c.png][]

不同方法在KITTI数据中的评估结果

![145d793c74cfe8dc0a8e593cbb375bcd.png][]

不同的法向量估计方法对里程计精度的影响

![361482fccd9fef57f1586d7bc51b761c.png][]

在KITTI数据中不同序列的轨迹示意图

![d361992596988e19607b2ced1a3f44eb.png][]

算法不同的模块耗时(注仅球形投影部分运行在CPU，其余算法的部分全部的运行于GPU)

点击**阅读原文**， 即可获取本文下载链接。

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

**3D视觉精品课程推荐：**

[1.面向自动驾驶领域的多传感器数据融合技术][1.]

[2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)][2._3D]  
[3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进][3.]  
[4.国内首个面向工业级实战的点云处理课程][4.]  
[5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解][5._-_-IMU-GPS_SLAM]  
[6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦][6._-_SLAM_VINS-Fusion]  
[7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化][7._LOAM_3D_SLAM_]  
[8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)][8._SLAM_cartographer_LOAM _LIO-SAM]

[9.从零搭建一套结构光3D重建系统\[理论+源码+实践\]][9._3D]

[10.单目深度估计方法：算法梳理与代码实现][10.]

##  ##

**重磅！****3DCVer-****学术论文写作投稿** **交流群****已成立**

扫码添加小助手微信，可**申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。**

**同时**也可申请加入我们的细分方向交流群，目前主要有**3D视觉**、**CV&深度学习**、**SLAM**、**三维重建**、**点云后处理**、**自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计**等微信群。

一定要备注：**研究方向+学校/公司+昵称**，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。**原创投稿**也请联系。

![b458852e346e782bdda47cf1750656d1.png][]

▲长按加微信群或投稿

![7faac471b9fb5f8cd85ecca2bfb93521.png][]

▲长按关注公众号

##  ##

**3D视觉从入门到精通知识星球**：针对3D视觉领域的**视频课****程（[三维重建系列][Link 1][三维点云系列][Link 2][结构光系列][Link 3]、[手眼标定][Link 4]、[相机标定][Link 5]、****激光/视觉SLAM****、****自动****驾驶等****）、****知识点汇总、入门进阶学习路线、最新paper分享、疑问解答**五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

##  ##

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

![bd443d543437421a9278235bd46c36d5.png][]

圈里有高质量教程资料、答疑解惑、助你高效解决问题

**觉得有用，麻烦给个赞和在看~**

[28c96400f7409f044b780c56091d953d.png]: /images/20220828/2a17a8e6fbde4b139ba9e52fee317b01.png
[c1407f6963770554daeda37d65d51536.png]: /images/20220828/57603d4d69c54a578edf9f3cce74f190.png
[d9bf6a9a703fd81b9ad0d23c3a540c7c.png]: /images/20220828/e48233fe2c504beea715f1e12d295d60.png
[fb635a8e08cd4464162b2e246a362da3.png]: /images/20220828/fa7fcee2164d452e91e171af9fe46823.png
[00c799e2351fc6f71b26bdf9468353a8.png]: /images/20220828/8a15cf7dac384b1d9ae4f250ac989277.png
[184a729ded5d9db2a8ec93d6d4c72639.png]: /images/20220828/652f5dad055947bc9289a7351c382868.png
[5f4b2674e8e164e15c2f3f52e57f39fb.png]: /images/20220828/dca15e502ea44e70a091d00c32fc6f9a.png
[e150ea2f4544d47e6073eec507e87412.png]: /images/20220828/e251fd8eb8f64a8cafff41daa31c3616.png
[0312fd998227b6a59d0c951b006ed23c.png]: /images/20220828/85decf90b33a4b8b99ee9b6286c73514.png
[a3b2c26014ff56e4d41592bcc5beca57.png]: /images/20220828/f1d56e2100424130b51dca3cda25fbe5.png
[75c68ecf05740fd6bde6d5f6f615096d.png]: /images/20220828/1afba5edcc234e5fb2d90aeeb8368fe0.png
[4aede2400e55caac4b46c16c46797ca9.png]: /images/20220828/d9bebac4137b49dd88006e6e21954a38.png
[cf2f43ac156529b8acbc1590a83a6a81.png]: /images/20220828/801f89b5e5204c8991cdc7b9022fbd63.png
[5f57da5ba21b05ae7bfc87fc9daeb349.png]: /images/20220828/cc3190c6ce574dd9b020ef202ff07119.png
[2527aa0852269e6549694625316b648f.png]: /images/20220828/e8243e36c36f4dafa0b384854963cb17.png
[cea4b1f5bfcb298f2d939307898ed5b7.png]: /images/20220828/631e6dbc2e05442588d8eecbd08dabb4.png
[469f95c350c65fa2e9ee814a1761167c.png]: /images/20220828/2ef1f440bbb249db8df5ad937c8aed88.png
[145d793c74cfe8dc0a8e593cbb375bcd.png]: /images/20220828/082373dfd40643edb30309f457ee2eb5.png
[361482fccd9fef57f1586d7bc51b761c.png]: /images/20220828/9174c3b826724516aec2de7de74a037a.png
[d361992596988e19607b2ced1a3f44eb.png]: /images/20220828/54b9e7ec56cf408eab6873fb0d6f8ab0.png
[1.]: https://blog.csdn.net/Yong_Qi2015/article/details/119284035
[2._3D]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247580753&idx=1&sn=174f87726a07cc67247c528de0e924ca&chksm=fbfd9765cc8a1e732c08cbaedaa78b35df43c1cb47ef94bad9ec1c942b9be63cf63a64e8c53f&scene=21#wechat_redirect
[3.]: https://blog.csdn.net/Yong_Qi2015/article/details/117828998
[4.]: https://blog.csdn.net/Yong_Qi2015/article/details/118865512
[5._-_-IMU-GPS_SLAM]: https://blog.csdn.net/Yong_Qi2015/article/details/119975746
[6._-_SLAM_VINS-Fusion]: https://blog.csdn.net/Yong_Qi2015/article/details/119047545
[7._LOAM_3D_SLAM_]: https://blog.csdn.net/qq_29462849/article/details/118448408
[8._SLAM_cartographer_LOAM _LIO-SAM]: https://blog.csdn.net/Yong_Qi2015/article/details/120328880
[9._3D]: https://blog.csdn.net/biaoshu2/article/details/106665145
[10.]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247582082&idx=2&sn=cc10291ddfdf6b05bdae71887c7c819c&chksm=fbfdaab6cc8a23a020a7359eae081f06c24572a12975bfd90396ac583a0081750f489db89b49&scene=21#wechat_redirect
[b458852e346e782bdda47cf1750656d1.png]: /images/20220828/b078c221e51b48519f9663c27d0d557d.png
[7faac471b9fb5f8cd85ecca2bfb93521.png]: /images/20220828/1ad4f442dd994a90a0f8ef308b39c9d7.png
[Link 1]: https://blog.csdn.net/DDWDDZY/article/details/105608179
[Link 2]: https://blog.csdn.net/pikachu_777/article/details/82992991
[Link 3]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247546284&idx=2&sn=d067a98047192a04f21489aca338a5d6&chksm=fbfc1e98cc8b978e5fb6b2afd1a473c89e2861374542421b9972f87a6fdfcd47eb50c443042e&scene=21#wechat_redirect
[Link 4]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247554670&idx=2&sn=cf3323178d5d1960476208e6303a8479&chksm=fbfc3d5acc8bb44c184084dcf447a5981d297815d62bef699129338d99a0472c622f96ee71d8&scene=21#wechat_redirect
[Link 5]: http://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247486422&idx=1&sn=381ab793959aed0eb6f01019dea522d4&chksm=fbff20e2cc88a9f456b80e23d2482fb319b08e89da071b39597bc51942a6aa7b756a6749856c&scene=21#wechat_redirect
[bd443d543437421a9278235bd46c36d5.png]: /images/20220828/58913693b1db4b72aff249f131fc836a.png