在上一篇里我们大概对VIO中的初始化方法进行了讨论,接下来我们对于OpenVINS中的动态初始化进行分析,主要参考一些文章:
[1] Closed-form Solutions for Vision-aided Inertial Navigation
[2] OpenVINS State Initialization: Details and Derivations
目录
- IMU 传播模型
- 相机观测
- 构建Ax=b
- 问题求解
- 恢复所有初始化变量
- 最大似然估计
- IMU Factor
- Camera Factor
- Prior Factor
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
这块不是很理解,先贴上官方的解释吧