COIN-LIO是解决纯雷达退化的终极答案？-优快云博客

作者 | Neob0dy 编辑 | 3D视觉之心原文链接：https://zhuanlan.zhihu.com/p/697885897

点击下方卡片，关注“自动驾驶之心”公众号

本文只做学术分享，如有侵权，联系删文

此工作利用激光雷达观测的反射强度作为“灰度”，经过滤波后构建投影图像，通过图像到图像的直接法以提供退化情况下的有效观测。代码开源好文明：https://github.com/ethz-asl/COIN-LIO

COIN-LIO: Complementary Intensity-Augmented LiDAR Inertial Odometry摘要翻译：本工作将 LiDAR 强度信息与基于几何的点云配准紧密结合起来。我们工作的重点是提高激光雷达惯性里程计在隧道或平坦场地等几何退化场景中的鲁棒性。我们将激光雷达强度返回投影到图像中，并提出了一种新颖的图像处理方式，该流程可生成经过滤波的图像，并提高图像内以及不同场景之间的亮度（强度）一致性。我们有效地利用强度作为一种附加模式，使用我们的新特征选择方案来检测点云配准中无信息的方向（无法约束的退化方向），并明确选择具有补充图像信息的补丁。然后，图像块中的光度误差最小化与迭代扩展卡尔曼滤波器中的惯性测量和点到平面配准融合。所提出的方法提高了在开放数据集上的准确性和稳健性。我们还发布了一个新的数据集，该数据集记录了具有挑战性的几何退化场景中的五个现实世界环境。通过使用额外的“光度”信息，我们的方法在所有比较的baseline都失败的环境中显着提高了针对几何简并性的鲁棒性。

1.太长不看版

背景

目前激光SLAM的主流范式是point2plane或NDT（广义icp）配准，即利用点云的几何信息提供对位姿的约束。但在环境几何出现退化场景如隧道、长走廊，或是面阵雷达遇到平面时，基于距离测量+点/面到面/概率分布的配准将无法约束一些方向上的运动自由度，使得里程计发生漂移。一些先前的工作尝试对观测进行分解，得到欠约束的方向并将其删除，防止错误的观测使得欠约束的方向的估计恶化，但这仍然没有解决这些方向可能出现漂移的问题。之前也有使用点云回波强度和反射率信息提高LOAM的估计准确度的工作，但并没有考虑到和几何信息观测的互补，并且生成的“亮度”图质量较差（空洞/点的位置不准确等，见详细阅读部分）。本文算是在一定程度上解决了这两个问题。

根据光路可逆原理，激光雷达和相机获取的数据都可以看作图像，只不过激光雷达的“像素”之间不是紧密排列而是分隔了一段距离。在BEV感知中已经大量运用了点云在球坐标下的投影图：

这是点云地图投影得到的图像，单帧扫描是没有这么致密的

可以认为本工作是Fast-LIVO/R3-live的纯Lidar版，直接用雷达回波强度构造了“伪image”。

方法概述

LIO部分和Fast-LIO2相同，本文主要的贡献是提出了基于雷达回波强度的“光度”误差配准模块。该模块会选择那些没有几何约束信息方向上的强度特征形成互补的姿态估计。整体pipeline如下：

dash表示姿态更新之前进行，实线是更新之后

预处理老三样，利用imu对点云去畸，并使用imu积分作为位姿初值，对点云进行降采样。同时上图右侧把当前帧扫描点云投影到球面坐标下再展开成0-360度的长条（宽度为FOV），其像素值为回波强度，然后对图像进行降噪与强度归一化处理。根据中间的geometric residual的观测结果进行奇异值分解（文中是对做PCA）可以判断出约束弱的方向。然后从图像中选取那些在几何约束弱的方向上 强度约束较强的特征块（即沿雷达退化方向平移/转动一点，就会导致图像的特征patch移动/旋转很多）构建光度误差函数（即视觉的direct method），拼接到几何约束观测的后面形成最终的观测H从而达到完整约束的目的。对于右下角的强度图像的特征管理模块针对召回率、遮挡、最大维护时间等进行了专门处理。

总结

很好的思路以及细腻的工程处理，对投影强度图做了很多工程上的改进，从而达到较好的鲁棒性，在退化场景上只通过雷达就能handle。当然硬要杠说如果隧道或者长走廊的材质均以没有任何纹理那也没办法。结合本工作+point-LIO的分段式高频更新&约束判断思路，应该可以处理大部分场景了。不过由于雷达的球极投影分辨率较低，对于线数少的雷达来说这个误差可能还是稍大，但至少能保证不飘，最后实验的结果还是很不错的，另外代码开源，非常好文明。

当前工作由于投影图的原因只适用于重复式扫描的机械式雷达，可以follow一下搞个非重复，分辨率不仅高还稠密。最好能结合360线扫式雷达+面阵雷达&相机，发挥各自的优势。

2.详细阅读

LIO部分跳过，参考Fast-LIO(2)即可。

强度图投影对于机械式360°雷达，将扫描点投影到横坐标为角度，纵坐标为FOV的图像上（展平）：

r是激光收发器距离电机中心的距离，很细节。

对于点，其图像坐标的计算公式为：

其中；为雷达水平和竖直方向的分辨率（不是角分辨率）。

本文的前作RI-LIO分析了激光雷达采集数据时的误差，机械式线扫雷达的两条线之间的角度通常被认为是定值，但由于制造加工误差导致实际角度并不是确定的（即不符合标称值），这会导致球投影图像出现空洞。在RI-LIO中作者提出了用于标定不同线束之间角度的方法并保存为列表：，在投影点的时候对于计算出的，找到中的从而插值计算出亚像素的坐标。

半固态激光雷达（旋转棱镜）工作原理：全网首拆！理想激光雷达——禾赛AT128，如何实现真128线？

图像滤波和强度归一化

光束之间不规则的仰角间距会导致强度图像中出现水平线伪影。它们在几何结构丰富的场景中不太明显，但在结构很少的环境中成为主体。由于它们以常规行频率出现（即呈现为投影图像中的水平线束，如下图），作者设计了一个有限脉冲响应滤波器（FIR，即计算机可实现的滤波器）来消除它们。

首先，在竖直方向使用高通滤波器，其截止频率略低于水平线伪影频率。除了水平线伪影之外，原始图像中的高频信息也会包含在高通信号里，高通信号称作A。此时，对A应用水平方向的低通滤波器，得到的就只剩伪影线条信号B了，因为伪影线条在水平方向上几乎保持不变是低频信号，而图像中的有用信号则是高频变化的。最后，我们从原强度图像中减去B信号，就可以将伪影滤除（当然若原图存在和伪影频率类似的信号也会不可避免地损失）。

由于强度值取决于表面的反射率以及距离和入射角，因此距离传感器越远、入射角越大的区域强度越低，但我们希望图像拥有不变性（类比视觉中的亮度不变和对比度不变）。而整个图像的强度变化较为平滑，这是因为平均距离和入射角通常由场景的全局结构而不是小的几何细节驱动。因此，可以使用一个大的平均窗口对图像中的强度值进行滑窗处理，得到平均的强度图。为了在整个图像中实现一致的“曝光”，在最终的强度图上对每个像素进行标准化，并使用常数因子将它们缩放到 [0, 255] 之间：，此处I(u,v）是原始强度。最后再对做3x3的高斯平均以去噪。

下图描述了算法处理前后的对比：

(1)：强度图像中心曝光过度且两侧曝光不足（远近导致的，走廊深处因为入射角大+距离远导致强度低）

(2)：滤波后的图像具有一致的亮度。

(3) 和 (4)：草地 (3) 和隧道 (4) 的细节视图。反射率（reflectivity）图像“曝光”不足，未显示(4)地面上的纹理。而纯反射强度（intensity）受到线条伪影很大的影响(3) 。施加文中设计的滤波器后消除了线条伪影（intensity w/o）。亮度补偿可产生一致的曝光并在更大范围内显示细节（(4) 中的地面纹理清晰可见，(3) 中的草地纹理也得到恢复）

图像特征选取

特征使用5x5的像素块表示（patch），这相比单个像素来说更加鲁棒（显然运动时雷达几乎没有可能两次扫到同一个点，这也是Fast-LIVO相对于R3live较大的改进）。受到DSO（经典的半直接里程计）的启发，选择图像梯度幅值高于阈值的候选像素作为patch center，并执行指定半径内的非极大值抑制。

通过筛选的候选patch大多是在三维场景中的形状不连续处(如边缘和角落)或表面反射率的变化(如地面材质变化或植物的叶子)上检测到的。形状不连续性上的像素强度雅可比的信息往往与几何点云配准中的信息重叠（颅内仿真一下位于边缘和角点的point2plane配准，一旦发生运动这些点附近平面的法向量就会剧烈变化）。因此，我们的目标是选择提供额外信息（几何约束缺失方向上能够进行光度约束）的patch。

首先对point2plane观测的Hessian 进行PCA。约束成分三个方向值若小于阈值认为该方向缺少信息。记缺少信息的方向集合。如果所有的方向都有充足的信息，沿着主成分的约束稍少的方向插入向量（例如有一个方向约束强另外两个稍弱，那就把另外两个方向向量加入，模长为与最长向量长度的差，方向保持不变）使得三个方向的约束都均衡一些。关于观测信息的分解详见X-ICP。

随后对每个patch中的每个点（特征追踪与管理中会讲为什么）计算图像的二阶矩（patch质心），并使用二阶矩 M 的最大特征向量来近似patch梯度，这个信息比直接使用像素梯度更稳定。计算投影图像坐标如何变化，如果该点沿着一个方向使扰动：

即假设一个雷达点沿着几何约束弱的方向上移动一小段距离，该点对应的图像patch在方向上会变化多少。投影模型参照公式。为了计算每个图像patch的贡献（应该让能够约束欠几何约束方向上的patch多出力），计算，这里之所以对长度做归一化是由于投影梯度的幅度随着patch与传感器距离的减小而增加，这会使得结果倾向于选择靠近传感器的patch，因此使用归一化梯度代替，从而更均匀地选取patch。

特征追踪与管理

使用当前的姿态估计在全局位置初始化一个patch中的每个点。不同于SVO中一个位置分配给整个patch，这允许COIN-LIO分别投影块中的每个点（见上一小节），从而捕获细粒度的细节。为了减少计算量，限制了跟踪的特征的数量。在每个更新步骤之后，评估patch的有效性。为了检测遮挡，比较块中每个点的预测更新距离和量测更新距离（即使用强度误差更新前后的distance误差/残差），如果差值超过阈值认为发生遮挡，丢弃块中的所有点。此外还剔除了小于最小范围或大于最大范围的图斑。

此外，计算跟踪的patch和新观测中的patch之间的归一化互相关( NCC )，如果NCC低于某个阈值，则将其移除。我们还限制了特征追踪的最大帧数（窗口），超过帧数限制就丢弃patch以减少误差积累，并鼓励新特征patch的初始化。另外，通过限制新特征和已经跟踪特征之间的最小距离来避免重叠特征。

这部分主要是工程细节，详细的看代码。

强度误差计算和卡尔曼量测更新

首先对当前雷达扫描用IMU进行点云去畸，然后将去畸后的当前扫描以及之前追踪的特征块投影到强度图像上并进行特征匹配和每个点的残差计算；在完成point2plane的残差计算和PCA后，根据缺少信息的方向给特征patch赋予权重，根据权重计算jacobian，分别为,,：

是强度图上的梯度（使用相邻像素值计算，注意是强度图上的2d点），是每个点对状态向量的梯度。公式具体就不推了，参考R3live的光度误差更新和SVO的patch误差计算。

最后把光度误差和点到面匹配误差拼接，光度Jacobian和几何jacobian拼接，得到完整的观测方程（当然需要加一个系数来调制两者的权重，这里就在 R 里面调）：

第一和第二个式子里的应该是原文的typo，实际上对应的是图像特征选取小节提到的每个patch的权重。

① 全网独家视频课程

BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、大模型与自动驾驶、Nerf、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

网页端官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

国内最大最专业，近3000人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型、端到端等，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频