VINS-初始化

• Initialization
  • 陀螺仪偏置初始化
  • 速度、重力向量和尺度
  • 重力refine
• 实验一
• 实验二
• 实验三

Initialization

本文的方法是，将imu的信息，与视觉的对齐，完成初始化

陀螺仪偏置初始化

这里的变量只有陀螺仪的偏置。见实验一中， 将bias通过自己的方法求出来。同时在公式11中，当把这个近似公式求出来的时候，就不用反复求积分了，而是求出来一次，然后下一次迭代就是根据这个更新来了。

公式14其实就是说的这个意思。

速度、重力向量和尺度

为什么要估计速度和重力向量？
预积分中速度的增量，是以bk为基准的，这里这里首先将其转换到c0坐标系下.$R_{c_0}^{b_k}{\hat{\beta }}_{b_{k+1}}^{b_k}$ --> c0坐标系下速度的增量.

c0坐标系下k+1时刻的速度， 加上重力加速度*时间，减去k时刻的速度。这个公式是从公式3来的。具体可以参考：https://blog.csdn.net/qq_41839222/article/details/89106128
见实验2；

重力refine

重力的大小是一个强约束，但是直接在前面加的话，就变成了一个非线性问题，不好解，这里将其norm作为一个额外的约束项加入进去。
根据实验3的结果，的确是不好收敛，主要是因为是一个norm的约束。符合vins论文中说的“it will become nonlinear and hard to solve.”

这里需要c0坐标系下的重力转换到惯性系下。且让相机的的yaw超前。

实验一

将陀螺仪的偏置设置为{0.1， 0.5， 1.0}，采用vins初始化的结果为：

这里尝试将这一部分改为优化:

ceres自动求导算出来的陀螺仪偏置与自动求导算出来的一样。

这里可以发现一个问题，当bg是变量估计的时候，这个预积分需要重复进行计算。论文中线性化部分的代码对加速是比较关键的。

比如这个地方迭代三次，那就需要把这个窗口内的预积分重新计算三遍。假设本窗口有1s，那么有200次的imu数据，那么就是600次计算。

实验二

将尺度的估计自己实现一遍；
TestInitScaleNode中实现。

与原文中略有差异，差别不太多。
可以看出，在相机坐标系下，重力是朝y方向向下的。再根据重力垂直朝下的特点，就可以求得惯性系下的相机初始pose。

实验三

当把grav的norm限定之后，发现的确不能很好的收敛了。

定位了若干个小时，这个问题的原因定位出来了，

template<typename T>
    bool operator()(const T* const grav, T* residuals) const {
    }

上面这个函数必须返回true，否则的话就会报evaluate 雅克比失败。

当把refine加上后， 收敛至9.8105. 但是发现重力方向的最后一个维度貌似有不同。后面如果有问题再来实验。但是的确是发现加上这个norm的约束之后，不太好收敛了。