OpenVINS代码解析-DynamicInitializer-（中）

在上一篇里我们大概对VIO中的初始化方法进行了讨论,接下来我们对于OpenVINS中的动态初始化进行分析,主要参考一些文章:
[1] Closed-form Solutions for Vision-aided Inertial Navigation
[2] OpenVINS State Initialization: Details and Derivations

IMU 传播模型

在进行初始化之前我们先来回顾一下IMU的传播方程:

根据IMU预积分的知识我们可以定义如下:

把全局坐标系定义为t0时刻的状态,那么将定义IMU预积分带入传播方程可得:

如果我们想得到每一帧相对t0时刻的状态,则可以得到如下:

相机观测

用如下表示在t0时刻IMU坐标系作为全局参考系下,将3D landmark反投到归一化坐标系下的过程:

其中:

这里符号 $$\wedge$$ 代表相机坐标系下的点投影到归一化坐标系下
我们这里假设相机的内外参数已知,则可以建立以下等式:

构建Ax=b

单个Landmark可能会被多个相机观测,我们将需要初始化的变量提出,上述式子可以变换成Ax=b的样式:

问题求解

通过求解上述问题,我们可以的到初始化速度/重力/特征点的全局位置,但是要注意对于重力来说模长是固定的,因此该问题转换成一个带约束的最小二乘求解问题:

使用拉各朗日乘数法(Lagrange Multiplier Approach)进行求解:

对于变量求偏导可得:

采用消元法进行求解,首先根据等式第一行可得:

代入第二行并进行整合可得:

将上述问题转换为如下的最小二乘问题对于x2进行求解:

根据文献A constrained eigenvalue problem可知,上述最小二乘问题与下面等价:

最终的问题转换为对拉各朗日系数进行带约束求解.根据文献A constrained eigenvalue problem可知为了上述问题有解,需要存在一个不为零的向量满足如下所示:

上述等式为quadratic eigenvalue problem, 关于quadratic eigenvalue problem定义如下:

那么根据eigenpolynomial可得:

通过上式可以求的拉各朗日乘法系数,那么就可得到变量x2(重力):

接下来根据上述定义可以的到x1,至此我们得到了速度/重力/特征点的3D位置:

恢复所有初始化变量

因为我们使用了一系列的观测进行初始化,因此我们需要恢复的不仅仅上述的变量,也包含各个观测位置/旋转/平移,如下所示:

接下来可以根据初始状态(红色)推导所有对应观测时刻的状态:

最后将t0时刻的重力与默认的重力方向进行对齐,使用Gram-Schmidt可以获得初始时刻相对于世界坐标系的旋转(注意这里yaw是不可观的):


那么可以将所有相对于t0时刻的状态全部转到世界坐标系下:

最大似然估计

通过上述过程我们基本恢复出了所有状态变量,但是我们已知系统是存在噪声的,而且我们希望将噪声也进行传播同时获得他的初始covariance matrix,定义优化的变量如下:

优化问题的cost由三部分组成:imu factor / camera factor/ prior factors:

IMU Factor

IMU Factor的定义与vins-mono类似,这里不再赘述,直接给出残差定义:

Camera Factor

每个特征观测可以写为如下格式:

Prior Factor

这块不是很理解,先贴上官方的解释吧