处理H自由度的不同方法

视觉-惯性状态估计过程中有四个自由度需要进行额外处理，它们是一个平移变量（3个自由度）和绕重力轴方向的旋转（1个自由度）。

视觉惯性状态估计最多可以达到四自由度的转换（围绕重力的旋转和平移），因此必须正确处理额外的自由度 。尽管在实践中使用了不同方法处理多余自由度，但没有任何研究来系统地分析它们的差异。本文介绍了在基于优化的视觉惯性状态估计中处理量测自由度的不同方法的对比分析。通过实验比较了三种常用方法：

1. 将不可观察的状态固定为给定值，
2. 在这些状态上设置先验
3. 在优化过程中让状态自由地优化。

实验表明：

1. 三种方法的准确性和计算时间相似，自由量测方法稍快一些；
2. 自由优化法的协方差估计似乎有很大的不同，但实际上与其他方法紧密相关。

引言

在机器人视觉研究领域。 惯性传感器与相机融合是互补的[1]，两者的结合可提供可靠而准确的状态估计。 尽管关于VI融合的大多数研究都集中在基于滤波器的方法上[2]，[3]，[4]，但非线性优化在最近几年中变得越来越流行。 与基于滤波器的方法相比，基于非线性优化的方法受线性累积误差的影响较小。 它们的主要缺点即高计算成本，已通过硬件和理论的发展得到缓解[5]，[6]。 最近的工作[5]，[7]，[8]，[9]在使用非线性优化的挑战性环境中显示了令人印象深刻的实时VI状态估计结果。

尽管这些工作具有相同的基本原理，即将状态估计作为非线性最小二乘优化问题求解，但它们使用不同的方法来处理VI系统中不可观察的自由度。众所周知，VI系统无法观察到全局位置和偏航[3]，[10]，在本文中，我们根据BA领域的惯例将其称为测量自由度[11]。给定这种测量自由度，获得唯一解决方案的自然方法是在优化中固定相应的状态（即参数）[12]。另一种可能性是将先验设置为不可观察状态，并且先验本质上在优化中充当虚拟度量[5]，[8]，[13]，[7]。最后，可以改为允许优化算法在迭代过程中自由更改不可观察的状态。尽管这三种方法在现有文献中都可以证明是可行的，但对VI状态估计的差异却没有进行比较研究：它们通常作为实现细节呈现，因此没有得到很好的研究和理解。此外，尽管已经研究了类似的仅视觉束调节问题（例如，在单目情况下具有7个不可观察的DoF的[11]，[14]），但据我们所知，此类研究尚未完成用于VI系统（具有4个不可观察的DoF）。

本文所对比的三种方法包括：规范自由度固定法，规范自由度先验法和自由规范自由度法。在准确性，计算成本和估计的协方差（该指标是SLAM感兴趣的[16]）。虽然所有这些方法在估计误差方面具有相似的性能，但由于收敛所需的迭代次数较少，因此自由规范方法略快。我们还发现，正如[7]所述，在自由规范自由度方法中，由优化产生的协方差与任何特定的参考系都没有关联（与规范自由度固定方法相反），这使得解释不确定性的含义变得困难。然而，在这项工作中，我们进一步表明，通过应用协方差变换，自由规范自由度方法得到的协方差实际上与其他方法所得的密切相关（见图1）。

问题描述

视觉惯性状态估计的问题包括推断相机-IMU组合传感器的运动以及传感器在场景中移动时摄像机看到的3D路标点的位置。 通过收集视觉测量（图像点）和惯性测量（加速度计和陀螺仪）的方程，可以将问题写为非线性最小二乘（NLLS）优化问题，其目的是最小化目标函数（ 例如，假设高斯误差）。
$$
J(\theta) = ||r^V(\theta)||^2_{\sum_V}+||r^I(\theta)||^2_{\sum_I}
$$
其中，$||r||^2_\Sigma = r^T\Sigma^{-1}r$ 是是残差向量r的平方Mahalanobis范数，$||r^I(\theta)||^2_{\Sigma_I}$是协方差矩阵，协方差矩阵用来加权。损失函数（1）可用在完全平滑[5]或固定滞后平滑[7]方法中。

方程（1）中的视觉项，包括测量图像点$x_{ij}$与米制重建得到的预测点$\hat x_{ij}$间的重投影误差。假设有针孔摄像机模型，则x̂ij（θ）∝ Ki（R> i | − Ri> pi）（Xj>，1）>，其中（Ri，pi）是第i个摄像机的外部参数（i = 0 ，…，N-1）和Xj是第j个界标点（j = 0，…，K-1）的3D欧几里得坐标。 我们假设固有校准Ki是无噪声的。 （1）中的惯性项由惯性测量值和IMU轨迹模型所预测的值之间的误差组成。 例如，[17]考虑了原始加速度和角速度测量中的误差，而[5]考虑了等效的较低速率测量中的误差（以视觉数据的速率表示的惯性预积分项）。 在这项工作中，我们将考虑后一种配方，尽管大多数结果并不取决于配方的选择。

假设有针孔摄像机模型，则$\hat{x̂}_{ij}（θ）∝ K——i（R> i | − R_i> p_i）（X_j^T，1）^T$，其中$（R_i，p_i）$是第i个摄像机的外部参数$（i = 0 ，…，N-1）$和$X_j$是第$j$个界标点$（j = 0，…，K-1）$的3D欧几里得坐标。 我们假设固有校准Ki是无噪声的。 （1）中的惯性项由惯性测量值和IMU轨迹模型所预测的值之间的误差组成。 例如，[17]考虑了原始加速度和角速度测量中的误差，而[5]考虑了等效的较低速率测量中的误差（以视觉数据的速率表示的惯性预积分项）。 在这项工作中，我们将考虑后一种描述，尽管大多数结果并不取决于描述方法的选择。

问题的参数（也称为状态）如下：
$$
θ \dot{=} {p_i , R_i , v_i , X_j }
$$
包括相机参数（外参和线速度）和3D路标点。加速度计和陀螺仪的偏置通常表示在IMU框架中，不受坐标系的影响。 因此，我们从状态中排除了bias，并假设IMU测量值已被校正。 惯性和视觉测量模型的完整阐述不在本文的讨论范围内，有关详细信息，请读者参考[5]。

由于VI状态估计问题的不确定性和不可观性，导致没有足够的方程来求解唯一解。

A. 解歧义和几何等价

当解决VI状态估计问题的时候，有必要确定目标函数来确定内参的变换，例如：
$$
J(\theta)=J(g(\theta))
$$
特别的，g 有本征矩阵的形式定义如下：

是一个4自由度变换，由任意平移t∈R3和围绕重力轴ez =（0，0，1的任意角度（yaw）α∈（-π，π）旋转Rz = Exp（αez）组成 ）>。 Exp（θ

不变性（3）的主要结果是（1）没有唯一的最小值，因为有很多的组合可以得到相同的最小误差：轨道上的所有重构（6）的代价最小（见图2） ），仅区别于4自由度转换（4）。 因此，VI估计问题具有一些不确定性或不可观察的状态：没有足够的方程式来完全指定唯一的解。

图2：不同规范处理方法所采用的优化路径示意图。 量规固定方法始终在量规C上移动，因此满足了量规约束。 自由量规方法使用伪逆来选择给定成本降低的最小尺寸的参数步长，因此垂直于成本等值线（1）移动。 量规先验方法遵循量规固定和自由量规方法之间的路径。 它最大程度地减少了（11）所增加的成本，因此它可能无法完全落在最小视觉惯性成本（1）的轨道上。

三种不同解决方法的对比

三种处理方法

一般在状态估计过程中，采用以下三种方法来处理这几个不可观状态：（1）将它们设置成固定值（固定值法）；（2）为它们设置一个先验值，然后在估计过程中进行优化；（3）不进行特殊处理，直接进行优化（自由优化法）。

1.固定值法：在优化过程中，保持第一个相机位姿的平移变量（涉及三个维度）和旋转变量（李代数表示法）中的z分量不变。具体做法是：在优化过程中，设置
$$
p_0=p^0_0\Δϕ_{0_z}≐e_z^T Δϕ=0
$$
其中是第一个相机的初始位置。这样的设置也意味着Jacobi矩阵中，对上述变量的(导数）取值为0。

2.先验值法：在优化的时候，给目标函数增加一个惩罚项：
$$
||r^P_0||^2_{\sum^P_0} \ where, r^P_0(\theta) \dot{=}[p_0-o_0]^0，\Delta\phi_0z]^T
$$
于是惩罚项。当时，效果接近于自由优化法，当时，效果基本上接近于固定值法。

3.自由优化法：在优化的过程中，让变量随着优化的过程自行演化。为了避免hessian矩阵的奇异化，可以求矩阵的广义逆或者使用阻尼因子来保证优化过程有一个良好的迭代更新过程。

结论

结果表明

• 三种方法的数值精确度基本相同 先验值法需要选择合适的先验值权重以避免增加计算量
• 在选择了合适的先验值的情况下，先验值法和固定值法的表现（精确度和计算成本）基本相同
• 自由优化法比其他两种方法略微快一些，因为它用了更少的迭代来达到收敛
• 自由优化法的另一个优点是它更通用一些，不需要对旋转进行特殊处理

虽然可能可以修复不可观察的自由度（量身定制的参数化方法（9）来修复偏航自由度），但自由规范方法具有通用的额外优势，即不是VI特有的，因此 不需要对旋转参数化进行任何特殊处理。