[论文笔记|VIO]On the Comparison of Gauge Freedom Handling in Optimization-based V-I State Estimation

摘要

众所周知，V-I状态估计在四自由度(DoF)变换(围绕重力旋转和平移)下是可能的，而额外的dfs(H自由度)必须得到适当的处理。本文首次对基于优化的视惯性状态估计中处理H自由度的不同方法进行了比较分析，并进行实验以比较三种常用的方法。我们表明(i)三种方法的精度和计算时间相似，free gauge approach略快;(ii)从free gauge approach得到的协方差估计似乎有很大的不同，但实际上与其他方法密切相关。其研究结果在仿真和实际数据集上都得到了验证，对于设计基于优化的视觉-惯性状态估计算法有一定的参考价值。

I. INTRODUCTION

V-I传感器融合中，基于非线性优化的方法比基于滤波的方法更不容易积累线性化误差。但主要缺点是高计算成本(硬件和理论的进步已经有所减轻)。但两者基于相同的原理，也就是将状态估计问题求解转换为非线性最小二乘优化问题，采取了不同的方法处理VI系统中不可观测的自由度。

对于一个VI系统，全局位置和偏航yaw是不可观测的，在本篇论文中按BA中的约定，称其为H自由度。给定这个H自由度，得到唯一解的一种方法是在优化时固定相应的状态(即参数)[gauge fixation approach]。第二种方法是在不可观测状态上设置一个先验，这个先验本质上是优化的虚拟测量[gauge prior approach]。第三种方法，允许优化算法在迭代过程中自由地改变不可观测的状态[free gauge approach]。

虽然这三种方法在现有的文献中都被证明是有效的，但是对于它们在VI状态估计方面的差异并没有进行比较研究:。此外，虽然对于纯视觉BA的类似问题已经进行了研究(例如，在单目情况下，有7个不可观测的dof)，但还没有对VI系统(有4个不可观测的dof)进行过这样的研究。

在这项工作中，我们展示了对于不同的方法在基于优化的视觉惯性状态估计中处理H自由度的分析。我们比较了上文提到的这三种方法在模拟数据和真实数据上的准确性、计算成本和估计协方差。

图1.EuRoC MAV数据集(MAV向负x方向移动).关键帧的不同构成不确定性。左边的图显示了free gauge approach的不确定性(没有选择参考框架)。右图中，将参考系设置为第一帧，因此，不确定性随着VI系统的移动而增加。从这两种方法中，我们可以清楚地看出参数不确定性的不同。

文章结构：
Section II，介绍了基于优化的VI状态估计问题及其非唯一解。
Section III，介绍了处理H自由度的不同方法。
Section IV，描述了本篇论文比较研究的模拟设置。
Sections V and VI，在准确性/时间和协方差方面的详细比较。
Section VII，展示了真实数据集的实验结果。

II.问题公式化和不确定性

V-I状态估计的问题包括推断相机-IMU组合传感器的运动和当传感器在场景中移动时相机所看到的三维地标的位置。通过收集视觉测量(图像点)和惯性测量(加速度计和陀螺仪)的方程，问题可以写成非线性最小二乘优化方程，其目标是最小化目标函数:

其中，$||r|{|}_{\Sigma }^2=r^T{\Sigma }^{-1}r$，$\Sigma$是协方差矩阵。

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

III. OPTIMIZATION AND GAUGE HANDLING

从最优化的角度看，用高斯-牛顿算法对(1)进行最小化是有困难的。即使我们对状态(参数向量)$\theta$的所有元素使用最小参数化，驱动参数更新的(1)的Hessian矩阵由于不可观测的dof是奇异的。

有几种方法可以缓解这个问题，如表I所示。

①gauge fixation approach：在一个较小的参数空间中进行优化，在这个空间中不存在不可观测的状态，因此Hessian是可逆的。这本质上强制了解决方案的硬约束(规范固定方法)。
②gauge prior approach：用附加惩罚(产生一个可逆的Hessian)来扩充目标函数，以使解决方案以一种度量先验方法满足某些约束。
③free gauge approach：可以使用奇异Hessian的伪逆来隐式地为唯一解(free gauge approach)提供额外的约束(最小范数的参数更新)。

前两种策略需要VI问题特定的需要约束的状态，而最后一种策略是通用的。

A. Rotation Parametrization for Gauge Fixation or Prior

gauge prior approach和gauge fixation approach这两种方法的一个问题是，固定相机姿态的1自由度偏航旋转角并不简单，我们接下来将讨论这个问题。

更新方向变量的标准方法。在(1)的NLLS求解器(GaussNewton或LM)迭代过程中，使用局部坐标，其中，在第q次迭代时，更新为：

对于第一个相机$R_0$，我们使用更方便的参数化。我们不直接使用$R_0$，而是使用左乘增量:

B. Different Approaches for Handling Gauge Freedom

在前面讨论的基础上，在整个优化过程中，测量固定包括固定第一个相机位姿的位置和偏航角。这是通过设置来实现的:

其中$p_0^0$为第一个摄像头的初始位置。修复这些值的参数向量相当于设置相应的剩余的雅可比矩阵的列向量(1)为零,即$J_{p0}=0,J_{∆{\phi }_{0z}}=0$。

gauge prior approach是增加惩罚项：

free gauge approach使参数矢量在优化过程中自由演化。为了处理奇异Hessian，我们可以使用伪逆或添加一些阻尼(Levenberg-Marquardt算法)，以便NLLS问题具有定义良好的参数更新。

三种方法优化迭代过程中参数空间路径的比较如图2所示。

IV. COMPARISON STUDY: SIMULATION SETUP

A. Data Generation

在实验中使用了三个6自由度的轨迹，即一个正弦形状的，一个弧状的和一个矩形的。我们把它们分别表示为sine, arc and rec 。

两种地标配置:平面(三维点大致分布在几个平面上);随机(三维点沿轨迹随机生成)。图3给出了一些仿真设置示例。

图3.样本模拟场景。左图为随机生成的三维点的正弦轨迹，右图为分布在两个平面上的三维点的圆弧轨迹

为了产生惯性测量数据，使用b样条拟合轨迹，然后采样加速度和角速度。采样值加上偏差和高斯噪声，然后用作惯性测量。对于视觉测量，我们通过一个针孔相机模型投射三维点来获得相应的图像坐标，然后加上高斯噪声。

B. Optimization Solver

为了解决VI状态估计问题(1)，我们在Ceres求解器中使用LM算法。对于每个轨迹，我们沿着轨迹采样几个关键帧。状态向量包括这些关键帧的状态(位置，朝向和速度)和三维点的位置。初始状态由groundtruth随机分布。

C. Evaluation

1. 精度：为了评估估计状态的准确性，我们首先计算一个变换，以使估计和groundtruth保持一致。变换是从两个轨迹的第一个位姿开始计算的(变换有四个dof，即，一个平移和绕重力矢量旋转)。对齐后，计算所有关键帧的均方根误差(RMSE)。具体来说，我们用欧几里德距离来表示位置和速度误差。对于旋转估计，我们首先计算对齐后的旋转与groundtruth之间的相对旋转(以角轴表示)，然后使用相对旋转角度作为旋转误差。

2. 计算效率：记录求解器的收敛时间和迭代次数。运行每个配置(轨迹与点的组合)进行50次试验，计算平均时间和精度指标。
3. 协方差:估计参数的协方差矩阵由Hession矩阵的逆给出。对于
   free gauge approach，由于Hession是奇异的，因此使用the Moore-Penrose伪逆。

V. COMPARISON STUDY: TIMING AND ACCURACY

A. Gauge Prior: Choosing the Appropriate Prior Weight

1)准确性:图4显示了RMSE如何随着先验权重的变化而变化。可以看出，不同先验权值的估计误差非常相似(注意纵轴上的数字)。虽然对于轨迹和3D点的不同配置没有明确的最优先验权重，但当权重增加到某个阈值(如图4中的500)以上时，RMSE稳定在一个值。

2)计算成本：图5示出了不同先验权值的计算成本。与图4相似，当先验权值大于一定值时，迭代次数和收敛时间趋于稳定。有趣的是，当先前的权重从0增加到稳定的阈值时，在计算时间上会有一个峰值。

图5.不同先验权值的迭代次数和计算时间

B. Accuracy and Computational Effort

表二列出了50个试验的平均RMSEs。我们省略了the gauge prior approach的结果，因为它们与the gauge fixation approach在小数点后8位以内的结果相同。可以看出，free gauge approach和gauge fixation approach之间只有很小的差异，在所有的模拟配置中，两者的精度都不高。

收敛时间和迭代次数如图7所示。the gauge prior approach和gauge fixation approach的计算成本几乎相同。free gauge approach比其他两种方法稍微快一些。具体地说，除了具有随机三维点的正弦轨迹外，the free gauge approach的迭代次数更少，收敛时间更短。注意，由于优化中变量的数量较少，每次迭代使用的时间最少(见表I)。

C. Discussion

根据本节的结果，我们得出结论:

• 这三种方法具有几乎相同的准确性。
• the gauge prior approach需要选择合适的先验权重，以避免增加计算成本。
• 在适当的权重下，the gauge prior approach与gauge fixation approach具有几乎相同的性能(精度和计算成本)。
• free gauge approach比其他方法稍微快一些，因为它需要更少的迭代来收敛。但是free gauge approach有一个额外的优点，那就是它是通用的,而不是特定于VI，因此不需要对旋转参数化进行任何特殊处理。

VI. COMPARISON STUDY: COVARIANCE

来自free gauge approach的参数协方差与其他方法不同，不能直接以有意义的方式解释。可以通过线性变换(12)将free gauge approach的协方差转化为gauge fixation approach。协方差变换方法VI-B节,这是一个特殊情况的一般理论,如果优化方法作为黑盒，可以用于协方差计算(即不能直接从测量模型的雅可比矩阵计算海森矩阵的协方差逆)。