python夹角余弦雷达图_Loam livox：一种用于小视场角激光雷达的快速、鲁棒、高精度的激光雷达里程计和建图包...

点击上方“计算机视觉life”，选择“星标”

快速获得最新干货

标题：Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV

作者：Jiarong Lin and Fu Zhang

来源：ICRA 2020

编译：阮建源

审核：万应才，李鑫

摘要

  大家好，今天为大家带来的文章是 -- Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV，发表于2019年的IROS。

    由于基于激光雷达的里程计和建图(LOAM)具有即时获取机器人位姿和建立周围环境准确、高精度地图的能力，其在自主车辆中扮演着重要的角色。这让自主车辆的自主导航和安全航迹规划可行。在本文中，我们展现了一种针对小视场角(FoV)和不规则采样的激光雷达的鲁棒、实时的基于激光雷达的里程计和建图算法。通过在前、后端的努力，我们解决了数个此种激光雷达带来的基础性挑战，并且相较于现有的基准(A-LOAM)，在精度和效率上均取得了更好的表现。为了将我们的发现分享以对社群做出贡献，我们将代码在Github上开源。

主要贡献

    解决了许多关键问题，包括在非常有限视场角下的特征提取，鲁棒的野值剔除，动态物体的过滤和运动畸变的补偿。在没有如IMU、GPS、相机等其他传感器的情况下，通过将点云和特定范围内的局部地图进行配准，我们的算法可以实时的计算激光雷达的位姿(也就是里程计)。

算法流程

1、介绍

图1 香港大学Chong Yuet Ming文化中心的三维地图

图2 香港科技大学本部的大规模建图，上图和下图分别为鸟瞰图和侧视图。上图种，白色的路径是激光雷达的轨迹，点云根据高度着色。

  固态雷达给激光雷达的定位与建图带来了如下的挑战：以Livox MID40 LiDAR为例(译者注：Livox中仍然是通过旋转透镜的方式实现扫描，或许应不属于激光雷达)

1．小视场角(图3)

2．非规则的扫描方式(图4)

3．非重复扫描

4．运动模糊

图3 Livox Mid-40和Velodyne PUCK (VLP-16)的视场角

图4 三维点投影在前部1m远的平面上的扫描轨迹，颜色代表采样时间

2、点的选取和特征提取：

  本系统的概览如图5所示，其中，前端处理有点的选取和特征提取组成。考虑到激光雷达的测量机制和底层的物理特性(如激光光斑的大小，型号的噪声)，选取好的点。

图5 我们的工作流的概览。新的每一帧直接和全局地图进行配准来提供里程计输出。配准的结果依次用于将此帧配准到全局地图中，进而获取相同频率(例如，20Hz)的里程计输出和地图更新。在我们的实现中，只有特征点(例如，线点和面点)在内存中保存，并且原始点在硬盘中保存，以便于可能进行的离线处理(例如，离线的全局优化)。

A点的选取

图6 (a)入射角反射角的图解(b)线到线的残差(c)面到面的残差

  采用“前左上”对应XYZ轴的坐标系，对于每个激光点P=[x,y,z]计算如下的特征：深度：

折射角为X轴和激光束之间的夹角：

反射强度为：

其中，R是物体的反射率。

入射角为激光束和光斑所在局部平面的夹角：

  为了提升定位和建图的精度，移除如下点：

1． 视场边缘的点，其会有大的曲率

2． 太大或太小强度的点，其中，强度过大的点常导致接受电路的饱和或畸变，因此精度不搞，强度小的点信噪比低

3． 入射角接近于180度或者0度，光斑会被拉长导致测量不准

4． 遮挡点，如图6(a)判断标准为：

B特征提取

特征点和LOAM中相似，利用局部曲率计算，由于视场角有限，为了保证特征的数量，利用反射强度信息，若相邻点之间的反射强度区别较大，将其也认为是“线点”。这种方式可以利用材料的信息。

3、迭代的位姿估计

    由于非重复的扫描方式，无法像LOAM、LegoLOAM、In2laama等那样持续地跟踪所提取地特征点，例如，即使雷达是静止的，扫描轨迹也和此前帧不同。使用迭代的位姿估计，以达到实时的里程计和建图。

A线到线的残差

    利用采集此帧最后一个点时的位姿代表此帧的雷达位姿，将当前帧中的线特征点投影到世界坐标系中，从地图中的线特征点中提取5个点，利用协方差矩阵的特征值确认这些点属于一条直线(当最大的特征值是第二大的特征值的五倍以上时)，KD-tree在单独的线程建立。Pi为所选取的点，计算残差为：

B面到面的残差

    和线特征点类似，当前帧的面特征点也在地图中寻找对应的5个点，判断确实为平面后，计算残差：

C帧内运动补偿

图7 我们为多核CPU设计的双线程示例。每一个“子帧”在一个专门的线程上独自地和全局地图进行匹配。匹配后的“子帧”然后配准到全局地图，并且称为地图的一部分。另外一个专门的线程接受新配准后的“子帧”并且用更新后的地图建立一个KD tree用于下一帧。

有两种方式进行运动补偿：

1.     分段处理

    将当前帧划分为数个“子帧”，独立地和地图配准，投影时使用的位姿对应的是“子帧”末尾的位姿。多个子帧配准过程在多个线程上同时运行。尽管较为简单，但是结果非常好。

2.     线性插值

    一种常用的方法是线性插值，计算帧结尾和帧开头间的相对变换：

假定前一帧最后一个点的采样时间是tk-1,当前帧中，在t时刻采样的点，计算s=(t-tk-1)/(tk-tk-1),然后进行线性插值：

其中：

因此只要计算正弦和余弦值。当前时间的位姿为：

此时可以将点投影到世界坐标系中：

D野值的剔除，动态物体的过滤

    为了避免动态物体降低精确度，如下过滤：在位姿估计的每次迭代中，先进行小数量的迭代(如两次)，使用结果，再次计算残差，移除最大的20%。算法的流程：

主要结果

结果

A建图的评价

    图8展示了两种运动补偿的结果，不使用运动补偿时局部非常模糊，使用线性补偿时仍有误差，而分段效果最好，与数据使用手持数据采集有关。

图8 不同的运动补偿方式的对比。第一列展示了没有运动补偿的结果，第二列为使用线性插值，第三列为分段处理。第四列中的上图为我们用于采集数据的的手持设备，下图是所建图区域的RGB图像。

B里程计的评价

    将本方法的定位和GPS进行对比，如图9，两图结果分别为0.41%和0.65%。

图9 我们的两个数据集的定位精度：室外(上图)和室内(下图)。在每个数据集中，我们对比了里程计的结果和谷歌地图并且计算了途径轨迹距离。

    随后和运动捕捉设备进行精度对比，图10结果表明与真值非常接近，最大的角度误差为1.1°。

图10 我们的结果和运动捕捉设备(mocap)系统的对比，虚线是mocap的测量，实现是我们算法里程计的输出。

C运行表现的评价

    比较了本方法和目前的基准的计算耗时，使用的是搭载i7-9700K的台式机和搭载i7-8550U的DJI manifold2板载计算机。如表1中所示，受益于并行的“子帧”和特征匹配和Kdtree建立，本方法比基准快2~3倍。

表1 我们算法和基准每帧的时间消耗，其中“Desktop PC parallel”和“Onboard PC parallel”使用了三个线程进行点云配准。

D其他

    推荐到gitbub上查阅代码和其他结果。

结论

    本文展现了一种为小视场角激光雷达设计的LOAM。方法继承了标准的LOAM中的基础结构和技术(如特征提取，匹配，线性运动补偿)，但有着数个关键的贡献，例如点的选取，迭代的位姿优化和并行化。所设计的算法可以实时地运行里程计和建图，尽管获得了高精度地地图和定位，但是序贯的配准带来了漂移，可以进一步的优化。

Abstract 

LiDAR odometry and mapping (LOAM) has been playing an important role in autonomous vehicles, due to its ability to simultaneously localize the robot’s pose and build high-precision, high-resolution maps of the surrounding environment. This enables autonomous navigation and safe path planning of autonomous vehicles. In this paper, we present a robust, real-time LOAM algorithm for LiDARs with small FoV and irregular samplings. By taking effort on both frontend and back-end, we address several fundamental challenges arising from such LiDARs, and achieve better performance in both precision and efficiency compared to existing baselines. To share our findings and to make contributions to the community, we open source our codes on Github1.

从零开始学习三维视觉核心技术SLAM，扫描查看介绍，3天内无条件退款

早就是优势，学习切忌单打独斗，这里有教程资料、练习作业、答疑解惑等，优质学习圈帮你少走弯路，快速入门！

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、图像处理、检测、分割、识别、医学影像、GAN、算法竞赛等微信群，请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~

投稿、合作也欢迎联系：simiter@126.com

长按关注计算机视觉life

最新AI干货，我在看