OpenGV 2.0：基于运动先验的车载环视标定与SLAM系统

本论文提出了一种基于优化的视觉 SLAM 解决方案，适用于车载环视摄像系统。由于这些系统最初的应用场景，它们通常仅包含单个朝向特定方向的摄像头，视场之间的重叠区域极其有限。我们的创新之处在于设计了三个优化模块，分别用于：基于简单的双视几何进行外部姿态的在线标定、稳健的前端初始化以估计相对位移，以及采用连续时间轨迹模型进行高精度的后端优化。这些模块的共同特点在于，它们都利用了与乘用车运动固有的非完整性特性相关的运动先验信息。与以往相关研究相比，我们的优化模块在规避 Ackermann 运动常见的部分不可观测性方面表现出色。此外将这些模块集成到一个全新的环视摄像头 SLAM 系统中，该系统专门针对在城市环境中运行的 Ackermann 车辆的应用。所有模块均经过深入的消融研究，并在公开的、大规模、高挑战性的在线数据集上成功验证了整个框架的实用性。值得注意的是，在论文接受后，该完整框架计划作为 OpenGV 库的扩展部分开源发布。

使用环视摄像头系统进行定位与建图仍面临诸多挑战。首先如图 1 所示，该系统通常仅包含四个摄像头，分别朝向不同方向，视场重叠区域极其有限。这不仅增加了利用立体约束进行直接深度感知的难度，也使系统的外参标定变得更加复杂。其次为了尽可能完整地生成鸟瞰视图，仅依靠四个摄像头，它们需要具备大视场角，这导致图像平面上的畸变增大且变化较大。第三，由于 Ackermann 转向车辆常见的退化运动模式，尺度观测性通常较弱。此外，市售低成本摄像头往往存在高传感器噪声和滚动快门等不利因素。

图 1. 本文研究的车载环视摄像头系统示例

环视摄像头系统在部分自动驾驶（如自动代客泊车）中的应用，最早由 V-Charge 项目进行探索。该研究表明，这类感知系统可提供足够的信息，以在未知环境中实现在线定位与建图，并进一步支持基于视觉的自主导航，特别是在受控的低速场景下。本论文的作者持续在该领域进行研究，并最终提出了一整套完整的算法体系，以进一步缓解上述挑战。其核心思想是利用运动先验信息。众所周知汽车是一种非完整性运动系统，其可控自由度数量少于实际自由度。即使车辆姿态限制在 3D 流形（即三个自由度），其可控自由度仍然较少。简单来说，在正常路况下且无侧滑的情况下，汽车无法横向移动，其运动可近似为局部切平面上的圆弧轨迹。本论文的主要贡献总结如下，这些贡献均基于引入部分运动约束，以促进、稳定或优化非重叠环视摄像头系统的几何标定问题：

• 总结了完整的技术栈，包括摄像头到车辆的在线外参标定、稳健的车载多摄像头运动初始化，以及基于 B 样条的大规模非完整性轨迹优化。

• 提出了一个完整的系统，将所有模块集成在一起，并最终在大规模公开城市基准数据集上实现了高可靠性、高精度的实时定位与建图。

• 更新了现有的开源软件包，扩展了该框架，引入了本文提出的受运动先验约束的几何环视求解器和优化器。

系统概述

图 5. 环视 SLAM 系统概览，显示跟踪和后端优化线程执行的所有步骤

本文最终贡献是一个基于全景视图的同步跟踪与地图构建框架，该框架围绕前面章节中描述的两个核心模块构建：运动初始化方法以及基于样条的后端优化方法。该框架采用ORB特征，其跟踪与地图构建模块受ORB-SLAM框架的启发。系统包含两个工作线程：一个前端线程运行视觉里程计，另一个后端优化线程运行带窗口的束束调整。需要注意的是，框架中没有添加回环闭合模块。相反加入了一个可选但经过仔细设计的低质量GPS信号，以防止长期漂移累积。借助所采用的核心模块，该框架能够实现前端相对位移的高度可靠初始化，以及后端精确的、即时的车辆运动学感知优化。

在前端视觉里程计线程中，每个来自全景视图系统的摄像头首先独立处理，以提取每一帧中的ORB特征。可以选择使用OpenCV的原始版本或ORB-SLAM的修改版（旨在实现更均匀的特征分布）。一旦提取了特征，会进行暴力时间内摄像头匹配。不进行跨摄像头的立体匹配，因为相邻视场的重叠区域被认为不存在或至少过小且变形，无法可靠地提取和匹配特征。相反通过可选的GPS读取来恢复并保持度量尺度。

但并不是每一帧都传递给后端优化器。前端采用一个简单的关键帧生成机制，验证帧间差异的中值是否超过某个阈值。然后，采用计算相对变换并通过RANSAC发现所有内点对应关系。接下来需要区分两种情况，如果某个对应关系是新的（即没有先前的3D地标参考），则通过三角化生成一个新的3D地标。反之，如果存在参考，则仅通过最新添加的关键帧中的新测量扩展该地标的测量列表。此外现有地标用于一致地重新调整新旧关键帧之间平移位移的幅度。鉴于可能存在多个这样的地标，通过对所有缩放因子的中值进行运算来以鲁棒的方式进行此操作。

建图线程会在足够的新关键帧添加后触发新的优化轮次。优化从标准的束束调整实现开始，通过最小化重投影误差来优化关键帧位姿和地标。这一预优化可以为后续的样条优化提供可靠的初始估计。后续的优化采用了更高效、几乎不受约束的FSBA变体，以确保精确、平滑且运动学感知的估计结果。由于没有惯性读取或重力测量的存在，我们还在滚转角上添加了正则化项，以限制沿第四维度的漂移。最后，GPS数据项作为可选的位置信差项被添加，以减缓长期的位移漂移。运动学约束轨迹模型的引入需要谨慎的时间节点放置策略。这尤其重要，因为长时间轨迹可能包含较少转弯的部分，而其他部分可能需要更多的节点和控制点，以允许更高的非线性。

在我们的工作中，引入的自动节点放置算法，该算法考虑了一种简单的平均策略，依赖于输入样本的局部时间密度。该策略的优势在于一旦车辆超过某个速度，每一组新采集的全景图像都会触发新的关键帧，从而使控制点在车速较高的直线路段上更加分散，在转弯时则更为集中。然而这一策略的缺点是，在城市环境中，交通信号灯和人行道过道导致车辆速度与道路布局的复杂性不总是相关。当汽车完全停下来时，情况极端。在这种情况下，自动节点放置可能会在长时间停驶期间触发额外的节点，这会导致控制点的局部积累，从而在最终优化结果中产生严重的伪影。为了规避这一问题，前端添加了一个检测静止间隔的特殊模块。该模块通过检查所有特征匹配的帧间差异，并将其与非常小的阈值进行比较，来评估有多少比例的差异可以忽略。如果这个比例足够大，则认为相对于背景的运动不存在。静止间隔被记录下来，曲线参数化的内部时间线仅是原始时间线中有效间隔的连续拼接。如前所述，低质量GPS读取被可选地集成，以实现更强的定位鲁棒性。在使用GPS信号时需要小心，避免使用由于卫星信号接收数量变化或多径效应导致的GPS异常值。通过GPS读取的连续性可以有效地检测到跳跃。只有当GPS读取保持在预期的误差范围内时，才会添加GPS数据项。所提出的配置确保了可靠的、全局一致的地图构建和定位，即使没有回环闭合或附加传感器，这使得它能够轻松部署在现有平台上。该框架不依赖于繁琐的IMU标定或与车辆固有传感器（如车轮里程计）的连接。

方法概述

相机的内参标定是一个较为成熟的问题，本文专注于一种实用的在线方法，用于对视场之间重叠有限或无重叠的环视摄像系统进行外参标定。该方法返回相机到车辆的参数，这对于运动先验的应用至关重要。然而需要注意的是，该方法仅限于推导外参旋转。这一策略的合理性在于，相机的位置通常由工厂参数精确定义，其公差足够小，不会对基于多视角相机的车辆运动估计精度产生显著影响。其原理如下：

• 如果车辆未发生任何旋转，则车辆上的任意点（包括相机中心）都将经历相同的平移位移，并且不同视角之间的旋转将为单位矩阵。因此从相机视角测得的位移仅仅是外参旋转的函数，而与位置无关。

• 更一般的平面运动可以用阿克曼转向模型进行局部解释。车辆的位移可近似为圆弧，并且对于安装在包含非转向轴的垂直平面内的相机而言，问题可简化为 1 自由度（DoF）。此时相机的垂直或横向偏移不会影响感知到的相机旋转或位移方向。ICR（瞬心）的半径（即比例因子）仅对具有前向偏移的相机可观测。然而由于前向偏移与场景深度之间的比值通常较小，该比例的可观测性依然有限，并且错误的偏移假设导致的误差可能会被一般噪声所掩盖。

此外，使用工厂参数的另一个动机在于，相机安装点与测得的相机位移之间的相关性较弱，使得从自然数据中标定平移外参变得困难。此外平移参数通常在时间上较为稳定。因此如果需要更高精度的参数，推荐使用基于运动捕捉系统的离线标定方法。本文主要关注相机到车辆的旋转参数，这些参数需要在出厂后进行标定，并且由于冲击、振动或温度变化的影响，需要持续监测。

图 2：本研究中使用的坐标系、路标、测量值以及外参几何变换变量的符号表示。

旋转外参的优化

该方法的核心目标是通过车辆的 运动方向 来推导摄像头的旋转外参。优化过程主要包括以下几个步骤：

(1) 运动约束的建立

• 该方法首先从极线几何关系 出发，但并非直接使用它作为代数残差，而是采用 物理空间的误差，即对应光线在三维空间中的正交距离，来进行优化。

• 通过引入 摄像头到车辆的旋转矩阵，可以将 不同时间步长的车辆旋转 约束在一个全局优化框架下。

• 这样每个摄像头的旋转参数不仅受到自身图像对之间的约束，还能够利用多个摄像头的观测结果来优化整个系统。

(2) 前向运动方向的约束

• 车辆的 前向方向（y 轴） 可用于建立优化约束。

• 由于摄像头相对车辆的平移参数不影响车辆的前向运动测量，因此摄像头的运动轨迹可以用来估计前向方向，并用作优化目标。

• 该方法会检测车辆是否在短时间内 无显著旋转，如果满足条件，则车辆的前向运动方向可以直接用于约束摄像头的旋转矩阵，使其与车辆前向方向对齐。

(3) 旋转中心（垂直方向）的约束

• 当车辆发生 较大角度旋转 时，可以认为旋转轴主要沿着 车辆的垂直方向（z 轴），因此可以用此信息来进一步优化摄像头的旋转参数。

• 该方法通过检测 旋转角度超过一定阈值 的运动情况，从而确定车辆的旋转中心，并对摄像头的旋转矩阵施加约束，使其垂直方向对齐。

(4) 结构化环境的约束

• 在人造环境中（如城市道路），通常可以利用 垂直线特征（如路灯杆、建筑边缘）来进一步优化摄像头的旋转参数。

• 该方法通过检测图像中的垂直线，并计算它们在三维空间中的方向，从而提供额外的几何约束，使摄像头的 垂直方向 与真实世界的垂直方向对齐。

全局优化

• 该方法将上述 前向方向约束、旋转中心约束、结构化环境约束 结合在一起，构造一个全局优化目标，并采用 鲁棒优化方法（如 Huber 损失） 来减少噪声影响。

• 由于优化目标仅涉及 旋转参数，且采用 3 自由度的旋转向量（如 Rodriguez 向量） 进行参数化，使得优化计算更加高效。

• 该方法避免了复杂的 全局束调整（Bundle Adjustment, BA），仅依赖 相邻帧对的约束，因此计算量较小，适用于在线标定。

该方法的优势

• 无需使用校准板或特定标定目标，可以直接基于车辆的自然运动进行标定，适用于实际应用场景。

• 不依赖大规模 SLAM 解决方案，仅通过局部图像对计算旋转外参，降低计算复杂度。

• 摄像头间的转换矩阵（camera-to-camera transformations）可直接作为推导的副产品，无需额外计算。

• 避免了对难以观测的平移参数（摄像头安装位置）的优化，提高了鲁棒性。

• 能够自适应调整优化权重，在不同约束之间进行平衡，以确保最优结果

图 3：线段 Lk的解释平面，该平面由测量线段在单位球面上的归一化坐标定义。

稳定运动初始化（Stable Motion Initialization）

在相机标定的任务中，稳定的运动初始化是一个关键步骤。该部分主要探讨了如何在多相机系统中进行稳定的运动估计，特别是在相机视角重叠较少甚至完全不重叠的情况下。

1. 通过极线几何约束进行运动估计

文章首先介绍了在两帧图像之间，通过最小化极线误差来估计相机的相对运动（旋转和位移）。其中，极线误差衡量的是特征点在不同视角下不满足极线约束的程度。然而，如果仅优化单对视图之间的相对位姿，位移向量的尺度因子是无法约束的。因此，作者采用了标量三重积（scalar triple product）的方法，将极线误差转换为一个仅依赖旋转矩阵的优化目标，从而避免了位移尺度的不确定性。该优化目标的求解方式是最小化一个矩阵的最小特征值，该矩阵仅与旋转矩阵相关。这种方法可以有效计算相对旋转，但在多相机系统中存在一定的局限性，例如当旋转矩阵为单位矩阵（即无旋转）时，能量函数会变为零，即使存在真实的旋转。

2. 线性方法的局限性

作者指出，基于广义本质矩阵的线性方法在某些运动情况下会退化，特别是在以下两种情况：

• 纯平移运动：如果相机的运动仅是平移，没有旋转，那么传统方法可能无法准确估计运动。

• 沿圆弧的位移：在车辆等应用场景中，摄像机的运动往往是弯曲路径上的位移，这也可能导致估计退化。

由于车辆运动通常至少部分接近上述两种情况，因此线性方法在实际应用中并不可行。

3. 适用于非重叠多相机系统的平面运动估计

为了克服上述问题，作者提出了一种新的方法，用于估计多相机系统在平面上的一般运动。该方法的核心思想是：

• 在每个相机视角内分别求解极线几何问题，将其转换为一个一元的特征值优化问题。

• 使用已知的相机外参旋转矩阵，将每个相机的观察结果旋转到与车辆本体坐标系对齐。这样可以在所有相机之间建立一个统一的旋转参考系，使得所有相机的法向量可以用于同一个优化目标。

• 最小化所有相机的特征值目标，并联合求解一个全局旋转矩阵，使得所有相机的极线几何误差最小化。

在该方法下，每个相机的位移仍然是独立的，但旋转是全局共享的，因此可以通过最小化特征值来优化整体旋转。

4. 旋转优化

针对平面运动的情况，作者使用 Cayley 变换 进行旋转参数化。Cayley 变换的一个优势是，它可以用一个单一的参数来描述二维平面上的旋转，从而使得优化问题变得更加高效。最终，作者采用 非线性优化（如 Levenberg-Marquardt 方法）来求解旋转，并通过 采样 z 轴旋转 进行初始化，以确保优化不会陷入局部最小值。

5. 物体空间误差优化

为了进一步提高精度，作者引入了基于 物体空间误差（Object-Space Error） 的优化方法。该方法的基本思想是：

• 极线误差可能存在旋转-平移歧义，特别是在相机视野较小或是车载侧向相机的情况下。

• 通过计算 光线之间的几何距离，即计算每对匹配点所定义的两条光线之间的最小距离，可以得到更加稳健的位姿估计。

• 该误差可以被转换为一个基于特征值的最优化问题，类似于之前的旋转优化问题。

相比于传统的 两视图捆绑调整（Bundle Adjustment），该方法的计算量明显更低，同时能有效避免旋转-平移歧义带来的局部最小值问题。

6. 绝对尺度的恢复

在多相机系统中，单靠两帧之间的极线几何关系，无法恢复位移的 绝对尺度，因为相对位移的尺度因子是不确定的。为了恢复尺度，作者利用了 手眼标定（Hand-Eye Calibration） 约束，即：

• 在车辆本体坐标系中，多个相机的运动满足一定的代数约束。

• 通过已知的相机外参，可以将多个相机的位移关系转换为一个线性方程组。

• 使用 奇异值分解（SVD） 计算尺度因子。

然而，由于 非重叠多相机系统的本质特性，如果旋转矩阵是单位矩阵（即没有旋转），那么尺度因子仍然是不可观测的。但在一般情况下，该方法仍然可以稳定地恢复运动的绝对尺度。

后端优化

在本模块中，介绍了运动感知几何求解器的最终工具——后端优化器。首先，我们探讨了一些必要的前提，为后续介绍所有车辆运动模型感知目标函数做准备。这些目标函数通常是通过黄金标准误差（即几何重投影误差）来公式化的。

A. B样条

连续时间参数化方法在处理平滑轨迹或时间密集采样传感器时，已证明在运动估计中非常有价值。为此，可以使用多种基础函数，如快速傅里叶变换（FFT）、离散余弦变换（DCT）、多项式核或贝塞尔样条。在本文中，采用了Furgale等人推广的高效平滑B样条参数化方法。具体而言，采用了p阶B样条曲线来表示平滑运动，其中控制点决定了轨迹的形状，而B样条基函数则控制着曲线的光滑性。B样条曲线的初始化来源于一组离散的车辆姿态和图像时间戳。为了初始化，采用了Piegl和Tiller提出的样条曲线近似算法。

B. 非全向运动

地面车辆通常具有非转向的双轮轴，这使得其运动成为非全向（non-holonomic）的。这个运动约束体现在Ackermann转向模型中。具体而言，车辆的瞬时运动是围绕瞬时旋转中心（ICR）旋转的，该旋转中心位于非转向的双轮轴上。换句话说，车辆的瞬时航向与其速度方向是平行的。这种约束在纯视觉算法中已被利用，但通常仅基于对分段常数转向角的近似来实现：

• 前端方法：例如，Scaramuzza等人将车辆运动近似为平面上的运动，并假设平台具有局部常数的转向角。两个连续视图之间的轨迹被近似为圆弧，车辆的航向始终切向圆弧。用圆弧的内接角度和半径来最小化运动参数化。

• 后端方法：如Peng等人提出的，通过分段常数转向角或圆弧模型，车辆的相对旋转和位移需要满足某些约束，这些约束可以作为正则化项添加到常见的束束调整框架中。

这些模型仅是对车辆速度和瞬时旋转中心位置约束的近似，接下来将介绍如何使用连续时间参数化方法来强制执行车辆速度方向与车辆前轴之间的一致性，我们称之为R-v约束。

C. 非全向轨迹优化

通过使用样条来表示车辆的非全向轨迹，并通过连续时间对运动轨迹进行建模，我们在优化过程中强制执行R-v约束。这种方法可以直接表达车辆的航向，或者作为软约束对车辆的朝向进行正则化。应用这些运动学约束的硬约束或软约束可能会影响优化的准确性和计算性能，因此本文介绍并比较了多种不同的优化方法，开始于传统的束束调整（CBA）。

1) 传统束束调整（CBA）

传统的束束调整方法通过最小化重投影误差来优化姿态和地标的位置。非线性目标函数通过调整相机模型将物体从相机坐标系转换到图像平面，计算重投影误差。该方法主要优化姿态和地标，尽管采用了基于地标的优化，但仍然是一种简化的优化形式。

2) 带R-t约束的束束调整（CBARt）

在传统束束调整的基础上，添加了R-t约束，作为一对软正则化约束。此约束通过计算两个视图之间的相对旋转和相对平移来修正运动模型的偏差，从而提高优化的准确性。该方法的核心思想是通过束束调整框架进行优化，并将这些约束作为正则化项加入到优化中。

3) 带样条回归的束束调整（CBASpRv）

在这种方法中，结合了传统的束束调整和样条回归。通过在优化过程中逐步回归3D样条并将其应用于软正则化R-v约束，这种方法允许更精确地描述车辆轨迹，且不完全依赖于圆弧模型。通过样条回归，可以根据车辆的姿态变化调整轨迹的平滑度和方向。