手写VIO课后作业（五）

课后作业

基础题

第一问：完成BA求解器部分代码

运行结果如下：可以看到相机位姿有所偏移，位姿在零空间发生了漂移。

当将前两个相机的pose设置fixed时，可看出，相机第一帧优化后结果为(0,0,0)，与真实值相同而未发生漂移。

第二问：完成滑动窗口算法测试函数

提升题

第一问：总结论文

总结

在VI中，使用高斯牛顿法等来进行最小二乘优化是不可行的，因为它对应的Hessian矩阵由于不可观的自由度变得奇异，具体来说H有4个不可观的自由度。可以通过指定3D重建中的参考坐标系来处理以得到唯一解。具体的三种处理方法总结如下：
（1）fixed gauge：将不可观测状态固定到某些给定值，在优化过程中固定第一个相机的位置和yaw角。在比较小的参数空间中进行优化，在这个空间中没有不可观测的状态，固H是可逆的。
（2）gauge prior：给不可观的状态上添加先验，通过添加一个惩罚项，使得H矩阵可逆，相当于在目标函数中添加了一个由先验项组成的惩罚项。
（3）free gauge：允许不客观状态量在优化中随意变化，使用奇异的Hessian的伪逆矩阵来提供额外的约束（使用最小范数来更新参数），来得到唯一解，或者通过添加一些阻尼项使得最下二乘问题有一个良好定义的参数更新。

实验效果

（1）精度
文中比较了3种方法的所有关键帧的均方根差（RMSE），具体地说，就是平移量位移和速度误差采用欧式距离，旋转量误差采用与真实值的相对误差角。50次实验结果显示，固定gauge法与先验gauge法的均方根差基本相同（小数点后8位），自由gauge法也仅与前两种方法差异较小，并无优劣之分。
（2）计算效率
文中比较了3种方法的收敛时间和迭代次数。先验gauge法与固定gauge法基本相同，二自由gauge法则比其他两种要稍微快一些，例如，sin轨迹中，自由gauge法迭代次数和时间较少。
（3）协方差
文中比较了估计参数的协方差矩阵，即H矩阵的逆。先验gauge法与固定gauge法基本相同。固定gauge法的第一帧位置的不确定性为0，这是由于第一帧固定住了，随后的位置不确定性增加。而自由gauge法的不确定性分布在各个位置，这是由于位置并没有固定在任何参考系中，但是，若将自由gauge法的协方差转换到固定gauge法约束对应的参考系，则两种方法的协方差基本相同，在Frobenius范数下只有0.11%的相对误差。

结论

• 3种方法基本具有相同的精确度。
• 先验gauge法需要设计适当的先验权重来避免计算成本的增加。
• 选用适当权重的先验gauge法与固定gauge法的精确度和计算效率基本相同。
• 自由gauge法比其他两种方法稍快一些，因为收敛所需的迭代次数更少。
• 直接得到的自由gauge法协方差并无太多几何意义，但经转换后得到更多的信息，并可与其他方法进行比较。

第二问