VINS-Mono+Fusion源码解析系列（十六）：重力修正

1. 基于重力先验知识进行修正

（1）理论推导

​ 以上视觉惯性对齐做的是通过对每一组图像帧和IMU预积分之间的关系，求出每一帧的速度，枢纽帧的重力加速度和尺度。在求解时，没有引入重力的先验知识，而在大部分情况下重力的先验是已知的，因此需要利用重力的先验知识对估计出的枢纽帧处的重力加速度进行修正。

​ 取出前面已经求出的枢纽帧下的重力方向，将其大小设为先验值9.81，作为当前重力的大小和方向。由于重力大小不会发生变化，当方向变化时重力作用点一定是在球面上运动的，如下图所示。在球面的切平面上，将其分解为两个相互正交的向量$b_1$和$b_2$，二者的矢量和为重力运动的方向。由此可将之前的3自由度重力转化为2自由度重力，在迭代求解中不断优化$b_1$和$b_2$，来获取最优解。

​ 注：之前在估计枢纽帧处的重力$g^{c_0}$时有3个自由度，现在修正时引入先验后有2个自由度。在ORB-SLAM3中，将重力先验直接放到整体中基于最大后验估计进行优化，而在VINS-Mono中是分两步进行的。
 通过以上描述可知，枢纽帧重力加速度大小为先验的9.81(记为$||\check{g}||$)，方向为前面初始化得到的$\vec{g}$。那么通过迭代求解来调整重力的初始值为$||\check{g}||\cdot \vec{g}$，重力调整表达式可写为：$||\check{g}||\cdot \vec{g}+w_1{\vec{b}}_1+w_2{\vec{b}}_2$。其中$w_1$和$w_2$为待调整的系数。

​ 于是，位移预积分量的线性方程可转化为：
αbk+1bk−pcb+Rc0bk⋅Rbk+1c0⋅pcb=Rc0bk(pck+1c0−pckc0)⋅s−vbkbkΔtk+12Rc0bkgc0Δt2⇒αbk+1bk−pcb+Rc0bk⋅Rbk+1c0⋅pcb−12Rc0bkΔtk2∣∣gˇ∣∣g⃗=Rc0bk(pck+1c0−pckc0)⋅s−vbkbkΔtk+12Rc0bkΔt2BW其中,w1b⃗1+w2b⃗2=[b⃗1b⃗2][w1w2] \alpha_{b_{k+1}}^{b_k} - p_c^b + R_{c_0}^{b_k} \cdot R_{b_{k+1}}^{c_0} \cdot p_c^b = R_{c_0}^{b_k}(p_{c_{k+1}}^{c_0} - p_{c_k}^{c_0}) \cdot s - v_{b_k}^{b_k}\Delta t_k + \frac{1}{2}R_{c_0}^{b_k}g^{c_0}\Delta t^2 \\ \Rightarrow \\ \alpha_{b_{k+1}}^{b_k} - p_c^b + R_{c_0}^{b_k} \cdot R_{b_{k+1}}^{c_0} \cdot p_c^b - \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2||\check{g}||\vec{g} = R_{c_0}^{b_k}(p_{c_{k+1}}^{c_0} - p_{c_k}^{c_0}) \cdot s - v_{b_k}^{b_k}\Delta t_k + \frac{1}{2}R_{c_0}^{b_k}\Delta t^2BW \\ 其中,w_1\vec{b}_1 + w_2\vec{b}_2 = \left[\begin{array}{cc} \vec{b}_1 & \vec{b}_2 \end{array} \right]\left[\begin{array}{cc} w_1 \\ w_2 \end{array} \right] αbk+1​bk​​−pcb​+Rc0​bk​​⋅Rbk+1​c0​​⋅pcb​=Rc0​bk​​(pck+1​c0​​−pck​c0​​)⋅s−vbk​bk​​Δtk​+21​Rc0​bk​​gc0​Δt2⇒αbk+1​bk​​−pcb​+Rc0​bk​​⋅Rbk+1​c0​​⋅pcb​−21​R