VINS-Mono源码解析（五）后端: 紧耦合优化

VINS-Mono源码解析（五）后端: 紧耦合优化

1. 优化原理

a) 优化变量

$$\begin{equation} \begin{aligned} \chi &= [x_0,x_1,..., x_n, ~~~~~x_c^b,~~~~~\lambda_0,\lambda_1,...,\lambda_m]\\ 第k帧状态: x_k &= [p_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g] ~~~ 15维\\ 相机外参: x_c^b&=[p_c^b,q_c^b] ~~~ 6维 \end{aligned} \end{equation}$$
$\lambda_i$是第i个特征点的第一个观测对应的 逆深度. 所以总的状态的长度为 $15\times N + 6 + M$, $N$为滑动窗中frame的个数, $M$为特征点的个数.

b) 优化函数

minχ⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪∣∣∣∣rp−Hpχ