优化过程中H自由度的处理方法比较

注：本文是论文《On the Comparison of Gauge Freedom Handling in Optimization-based Visual-Inertial State Estimation》的阅读笔记，论文作者是Zichao Zhang, Guillermo Gallego, Davide Scaramuzza。

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

一、处理方式：

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

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

2.先验值法：在优化的时候，给目标函数增加一个惩罚项：
$${\Vert r_0^P\Vert }_{{\Sigma }_0^P}^{2}where{r}_0^P(\theta )\doteq [p_0-p_0^0，{\Delta \varphi }_{0}z{]}^T$$先验协方差矩阵${\Sigma }_0^P$通常取为${\sigma }_0^2$，于是惩罚项$\Vert r_0^P{\Vert }_{{\Sigma }_0^P}^2=w^P{\Vert r_0^P\Vert }^2$，$w^P=\frac{1}{{\sigma }_0^2}$ 。当$w^P=0$时，效果接近于自由优化法，当$w^P\to \infty$时，效果基本上接近于固定值法。

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

二、实验效果：

论文作者使用了Ceres求解器中的LM方法，在模拟数据和实测数据上对上述三种方法进行了试验，并对结果从精确度、计算开销和协方差等方面进行了对比。
下面是若干实验数据：

三、结论：

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