基于自运动中准确估计地平面法向量方法

本文介绍了一种新颖的轮式车辆地平面法线估计方法。在实际应用中，由于制动和不稳定的路面，地平面会动态变化。因此，车辆的位姿，特别是俯仰角，会从微妙到明显地波动。因此估计地平面法线是有意义的，因为它可以提高各种自动驾驶任务的鲁棒性（例如3D目标检测、道路表面重建和轨迹规划）。我们提出的方法仅使用里程计作为输入，并实时估计准确的地平面法线向量。具体而言，它充分利用了自车位姿里程计（自我运动）与其附近地平面之间的基本关系。基于此设计了一种不变扩展卡尔曼滤波器（IEKF）来估计传感器坐标系中的法向量。因此提出的方法简单而高效，并支持基于摄像头和惯性的里程计算法。通过在公共数据集上进行多次实验证明了其可用性和显著的鲁棒性改进。例如在KITTI数据集上，我们实现了0.39°的估计向量误差，达到了最先进的准确性。

准确地估计地平面法线对于自动驾驶应用的感知、导航和规划至关重要，这是因为车辆坐标系中的地平面由于制动和不稳定的路面而动态变化（见图1）。因此，车辆的姿态，特别是俯仰角，会从微妙到明显地波动。为了提高自动驾驶系统的鲁棒性，地平面法线被估计并编码在与视觉相关的任务中，包括3D目标跟踪、车道检测和道路分割等。例如，地平面参数在许多应用中用于多摄像头校准。它们还被用于估计地面上物体的深度信息，并为系统提供重要的绝对尺度信息。除了上述任务外，现有的基于图像的地图构建和鸟瞰图（BEV）感知算法也对地平面法线参数的准确性非常敏感。例如一些基于BEV的算法应用逆透视映射（IPM）与从图像平面到地平面的外参数，从而将像素从图像空间映射到BEV空间。

然而，在实时环境中准确地估计地平面法线是具有挑战性的，特别是在单目设置中。主要原因是地平面法线的微妙动态在图像空间中反映较小。传统方法通常首先估计单应性变换，然后将其分解为地平面法线和自身运动。最近，一些神经网络被提出，以在像素级同时估计深度和法线，具有光度和几何一致性。然而，这些基于图像的方法由于地平面法线动态与图像线索之间的松散联系而导致精度不足。此外，大多数先前的研究简化（或假设）移动车辆的地平面法线矢量是恒定的，这与实际情况相反。在实践中，即使路面看起来是平坦的，当车辆移动时，法线矢量也会轻微波动。例如，一辆四轮轿车沿着一条笔直的街道行驶，顶部安装有一个面向前方的摄像头。相机的俯仰角（相对于地面）通常以约1度的幅度波动。尽管这种动态在图像空间中几乎不反映出来，但在使用具有固定外参数的IPM进行图像投影后，可以在BEV空间中轻松观察到这种动态（见图2a和补充视频以获得更好的可视化效果）。

图1. 展示了前置摄像头在移动车辆上的典型动态运动。尽管车辆直线行驶，路面看起来足够平坦，但俯仰角（绕x轴旋转）实际上在大约1度的振幅内波动。当车辆遇到不完美的路面和减速带时，这种俯仰角的振荡会被放大。

地平面法向量

我们认为在车辆移动时，车辆参考系统中的地平面法线向量是振荡的。为了验证这一点，我们从KITTI [28]里程计序列＃00中选取一个剪辑进行说明。理论上，如果地平面法线保持不变，具有固定外部参数的IPM图像（例如，相邻帧之间的平行道路车道和边缘）应该是相似的。

图3，具有恒定地平面法线的IPM图像：道路边缘未正确对齐

然而，如图3所示，在具有固定外部参数的相邻帧之间，通过使用恒定地平面法线进行IPM后，道路边缘未对齐。为了探索这一现象，我们使用数据集的LiDAR点来计算地平面法线的地面真值（GT）。在此基础上，GT道路边缘以红色虚线标记。我们清楚地发现，大多数实际道路边缘未与GT正确对齐，超出校准1度以上。为了更全面地了解此类动态的统计信息，我们根据它们在横滚和俯仰方向上相对于GT的变化数量对帧数进行计数。最终的统计结果如图4所示。

图4. 俯仰和横滚不对齐的帧的统计信息

可以观察到俯仰和横滚角的平均变化约为1.2度和3.5度，分别。换句话说，地平面法线向量不是恒定的，而是在车辆移动时动态变化。类似地，表1呈现了所有KITTI里程计序列上横滚和俯仰动态的平均值。我们可以得出相同的结论：当车辆移动时，地平面法线不是恒定的（大约为1度）。这种不稳定性可能进一步影响自动驾驶任务的性能。

通过引入了一种简单而高效的方法来估算车辆自运动中的地面法向量。特别是该方法与由各种传感器提供的SLAM（同时定位与地图构建）和SfM（运动结构）算法提供的自运动兼容（例如，单目摄像头和惯性测量单元（IMU））。为此，我们设计了一个不变的扩展卡尔曼滤波器（IEKF）来模拟车辆自运动的动态并实时估算地面法线。此外该方法可以轻松地嵌入大多数提供自运动的自动驾驶系统中，计算成本很低。正如图2所示，应用我们提出的方法后，图像质量得到了显著提高。简而言之，本文的主要贡献如下：

• 引入了一种实时地面法线估计的简单而高效的方法。

• 由于充分利用了自运动信息作为输入的特殊设计，所以所提出的方法支持基于摄像头和惯性的里程计算法。

图2，使用我们提出的方法之前和之后的透视变换图像比较。（a）来自KITTI里程计数据集的原始图像。（b）使用从相机到地面的固定外参的透视映变换图像。（c）使用我们提出的方法计算的动态外参的透视变换图像。可以清楚地观察到图（c）中的图像更为准确。

这里详细介绍了我们提出的地平面法线估计方法。图5展示了整个流程。简而言之，我们基于IEKF构建了从里程计（来自图像或IMU）到地平面法线的关系。为了更好地描述，以下描述以装在轮式车辆上的前置单目摄像头为例。

图5. 我们提出的地平面法线估计流程概览。提出的IEKF可以处理来自各种传感器的自我运动，例如IMU、来自单目图像的视觉里程计，以及可以提供实时里程计信息的SLAM系统。最终的地平面法线向量N是基于IEKF残差旋转和先前校准的静态外参的组合实时预测的。

对于移动的车辆，其摄像机姿势与地平面是显而易见的一致的。在实际环境中，道路表面并非理想的平面，但靠近摄像机的一小段近似平坦。在这种情况下，可以计算在摄像机参考系统中的该段的法线向量。当车辆静止时，可以从摄像机和地平面之间的外参参数计算地平面法线向量。可以通过脱机棋盘格标定轻松获取外参。当车辆移动时，由于横滚和俯仰角的振荡，外参不再准确地表示摄像机和地平面之间的关系。在这种情况下，触发提出的方法。

方法的原理是法线向量的动态特性在频域上可以粗略地分为两个部分：低频部分描述实际的高度变化，例如凸起和桥梁；高频部分是振荡，主要是由于刹车和加速。目标是从自我运动中分离这两个分量以计算地平面法线向量。总的来说，该方法基于两个假设：

（1）靠近摄像机的道路表面可以近似为一个平面。

（2）平均摄像机姿势接近其静态外参校准。

图6展示了两个相邻帧的摄像机参考系统，该方法使用IEKF模型建模车辆自我运动和地平面法线之间的关系，实时估计地平面法线。方法适用于多种传感器提供的自我运动信息，例如单目相机和惯性测量单元（IMU）。通过应用该方法，图像质量得到显着改善。在实验证明其有效性，估计的向量误差从[26]的3.02°降低到KITTI数据集[28]上的0.39°。

图6，两个相邻帧中的相机参考系统的2D侧视图

该方法采用IEKF（不变扩展卡尔曼滤波器）思想，在旋转估计场景中，直接在李群上使用确定性非线性观察器，而不是在线性输出上使用校正项。以自我运动为输入，通过IEKF输出地平面法线向量N，适用于单目SLAM系统、学习型单目里程计、纯IMU-based里程计以及其他实时自我运动提供的SLAM（或里程计）算法。整个过程在算法1中描述，采用零阶状态，即过程模型是输入旋转的单位函数。通过IEKF，状态和协方差初始化为零和单位矩阵，分别。方法的通用性表现在恒定的过程模型足以应对各种情况。观测是Tk的旋转部分，通过残差旋转（Gi）计算当前帧的法线向量（Ni）。在将当前帧的观察应用于滤波器之前，预测状态（Yi）与绝对变换（Tk）之间的差异会被计算。

实现

实验选择了两种具有挑战性的传感器设置，分别为单目摄像头和纯IMU里程计，并在流行的KITTI数据集上进行验证。对于单目设置，使用ORB-SLAM2算法从左侧RGB摄像头图像中获取自我运动信息。在纯IMU里程计方面，采用AI-IMU算法提取自我运动信息。实验中使用了LiDAR点云数据计算地平面法线的地面真实值。为了定量评估所提方法，使用LiDAR点云数据进行地平面法线的地面真实值计算。具体而言，通过将点云投影到图像上获得2D-3D对应关系，选择位于摄像机视觉包围盒内的点。然后，采用语义分割方法生成图像地面区域的掩码，最后通过RANSAC平面拟合得到地平面法线。对于IEKF，设置了过程方差的尺度参数。所有实验在配备Intel i5-6600K CPU的台式机上运行，无需使用GPU，操作系统为Ubuntu 18.04.6 LTS。

在定量评估中，通过与地面真实值的比较，使用弧度制的向量误差对估计的地平面法线向量进行了评估。研究采用了两种地面真实值类型，即固定外参和平面拟合。对于两种传感器类型（IMU和单目），我们的方法在精度方面都表现出色，实现了最佳准确性。例如，在KITTI数据集上，与现有方法相比，我们的方法将估计的向量误差从3.02°降低到0.39°。此外，基于单目的方法相较于仅使用IMU的里程计，提供了稍微更佳的结果。我们的方法还显著减少了计算时间，每帧在3–50毫秒之间，进一步证明了其实时估计准确地平面法线向量的能力。

表2. 我们提出的方法与之前研究的定量比较。这里还进行了运行时间的比较，以展示使用我们的方法提高效率的改进。特别是，采用的IEKF每帧的运行时间不到一毫秒

为了更好地理解我们的贡献，我们在图7中对比了使用静态法线向量（来自固定外参标定）和动态法线向量（来自我们提出的方法）的IPM图像。在这里，静态法线向量意味着地平面法线保持不变。理想情况下，如果在IPM中使用的地平面法线是准确的，平坦道路表面上的平行车道应该在IPM图像中保持平行。然而，如图7a所示，使用静态法线向量时，道路车道与静态法线向量不够平行。然而，使用我们方法得到的动态法线向量时，IPM图像中的道路边缘更平行和一致，如图7b所示。

图7. 使用(a)基于固定外参标定的静态法线向量和(b)使用我们提出的方法得到的动态法线向量的IPM图像的视觉对比。这里使用单目ORB-SLAM2形成里程计输入。我们可以清晰地看到，使用静态法线向量时，道路边缘不够平行。而基于我们方法得到的动态法线向量，IPM图像中的道路边缘更加平行和一致。

图8详细展示了KITTI里程计数据集-00中IPM序列的俯仰角变化。基于使用我们提出的方法得到的动态法线向量，我们可以清晰地看到在大多数帧中，俯仰角（单目相机和IMU都包括在内）与地面真值正确对齐。然而，在某些情况下（帧500到600），估计的地面法线与真值不同。原因是车辆正在进行急转弯，而在极端车辆动态下，使用IEKF的提出方法无法产生理想的估计，法线向量估计本质上等同于消失线估计。因此，将地面法线转化为消失线（在原始图像空间中）也可以为我们提出的方法提供令人信服的可视化。

图8. 使用所提方法计算的法线向量的俯仰角图。底部图显示了来自橙色框内50帧的详细信息。所提出方法计算的俯仰角的振荡趋势与地面真值非常一致。请注意，俯仰角的整体振幅实际上很小，通常在1度以内。

在图9中，绿线是从我们的提出方法中计算的，显示了合理的消失线估计。红线是从静态标定（静态法线向量）计算的，显然偏离了理想值。在补充视频中可以找到更好的可视化效果。为验证我们提出的方法的稳健性，我们在nuScenes数据集上进行了相同的实验。

图9. 消失线的可视化。红色和绿色水平线分别是从固定和动态地平面法线转化得到的消失线。底部图是来自顶部图像的橙色矩形区域的放大图像。绿线明显是对消失线更准确的估计。

如图10所示，左侧的图像是使用原始固定相机外参的IPM结果。右侧的图像显示了使用我们提出的方法估计的地平面法线的IPM结果。我们可以清晰地看到，所提出的方法产生了更稳定和合理的IPM图像。

图10. nuScenes数据集上的IPM可视化。

消融实验

为了评估在里程计中使用IEKF计算地平面法线的有效性，我们进行了额外的实验，仅使用里程计来获取地平面法线。直接使用里程计信息有两种方法：相对里程计和绝对里程计。前者是由里程计算法提供的相邻帧之间的相对姿势，后者是累积里程计，即相对于第一帧的当前姿势。

图11，仅使用里程计估计的地平面法线比较

如图11所示，仅使用纯相对里程计在某些情况下导致地面法线的不一致估计。这是因为帧之间的相对旋转仅包含车辆姿势的“瞬时信息”，因此无法处理各种道路表面，如小坡道或凸起。对于绝对里程计，由于里程计误差随着时间的推移而累积，结果甚至更糟。定量结果如表3所示。

局限性

虽然我们提出的方法可以实时估计准确的地平面法线向量，但仍存在两个局限性：（1）我们的方法只能应用于有轮车辆，因为它依赖于有轮车辆中地平面与自我运动之间的基础连接。（2）我们的方法依赖于附近的地平面始终可以近似为一个平坦的平面，并且车辆行驶平稳的假设。因此，如果车辆行驶在极不平整的道路上，如地形和斜坡，或者进行急转弯，估计精度可能会降低。对于这些情况，我们提出的方法估计的地平面法线的有效范围可能会缩小到更小的区域。

本文提出了一种在驾驶场景中估计地平面法线向量的方法。我们对车辆行驶时法线向量的动态特性进行了结构化研究，这些特性以前被认为是恒定的。通过可视化和定量实验证实了这一论点。在分析地平面法线和车辆里程计之间的基本关系后，我们采用不变的扩展卡尔曼滤波器以实时高精度地估计法线向量。滤波器的输入对产生里程计信息的传感器是不可知的。在公共数据集上的实验证明，我们的方法在单目和仅IMU里程计方面均取得了令人满意的精度。