LARVIO算法学习总结(一)-CSDN博客

本文链接：https://blog.csdn.net/j000007/article/details/136222920

1 前言

学习了一段时间的基于滤波的slam，其中参考了多位大神的博客，相关的博客链接也会放到文章的最下方，从中学习到很多，为此以larvio为例子总结一下滤波slam，文章较长，还请耐心看完。有些地方可能理解得还不是很到位，如有错误请指出交流。

2 LARVIO算法概述

LARVIO全称为Lightweight Accurate Robust Visual Inertial Odometry，本质上是一个基于Hybrid MSCKF的单目视觉惯性里程计。除了是一个Hybrid MSCKF之外，它还具备以下几个主要的功能：

适用于动态或静态的自动初始化

在线传感器参数标定

零速矫正

Hybrid MSCKF是将观测时间比较长的特征点增广到状态向量中，克服了MSCKF对于跟踪长度超过滑窗长度的地图点长时间约束的浪费问题。在Mingyang Li的最早版本中，都使用了3维（3D逆深度或3D世界坐标）的方式来对增广地图点状态进行参数化。一般也称它为Hybrid VIO或Hybrid EKF，最早是由Mingyang Li大佬提出的。同时，MSCKF是一个基于EKF的经典VIO方案。因此LARVIO本质上就是EKF，需要先弄清楚什么EKF，进而理解MSCKF。

MSCKF全称Multi-State Constraint Kalman Filter(多状态约束下的Kalman滤波器)，是一种滤波VIO。其目标是解决EKF-SLAM中维数爆炸问题，传统EKF-SLAM将特征点加入到状态向量中与IMU状态一起估计，当环境很大时，特征点会非常多，状态向量维数会变得非常大。MSCKF不是将特征点加入到状态向量，而是将不同时刻的相机位姿加入到状态向量中，并利用多个时刻相机共视形成的几何约束(Constraint)构建观测模型对状态进行更新。更进一步，为了控制状态规模，纯MSCKF仅维持固定数目的相机位姿，使用滑动窗口的方式来限制后端的计算量。而Hybrid MSCKF方案则还将观测时间较长的特征点加入状态向量，以提高定位精度。

2.1 IMU状态向量与INS

由于MSCKF本质就是一个EKF，所以介绍MSCKF之前，我们先介绍一下INS(Inertial Navigation System)中的IMU状态EKF估计，INS中IMU的状态向量为 $X_{IMU}=\begin{bmatrix}_{G}^{I}q^T & b_g^T & _{}^{G}v_I^T & b_a^T & ^{G}p_I^T\end{bmatrix}$ 其中

$_{G}^{I}q^T$ 为单位四元数，表示从世界系( G系)到IMU坐标系( I系)的旋转
$b_g^T$ 为加速度计accelerator的bias
$_{}^{G}v_IT $为IMU在G系下的速度
$b_a^T$ 为陀螺仪gyroscope的bias
$^{G}p_I^T$ 为IMU在G系下的位置

INS的EKF步骤为：

EKF预测：先利用传感器获得的观测加速度和观测角速度，可以对状态进行估计，显然，该步骤会使得估计的不确定度/协方差越来越大
EKF更新：然后利用GPS观测构建观测模型，对状态向量的均值和协方差进行更新, 修正预测过程的累积误差，减少不确定度。

2.2 MSCKF中的观测模型

对于MSCKF来说，EKF预测步骤与INS一样，区别在EKF观测更新，需要用视觉信息来构建观测模型，从而对IMU预测的状态进行更新。INS中GPS可以直接给出位置的观测，而视觉通常只能提供多个相机之间相对位姿关系的约束。利用特征点到相机的重投影误差（空间中一个3D特征点根据相机的姿态和位置投影到相机平面，与实际观测的特征点之间的误差）来进行约束：
请添加图片描述

误差=实际观测到的特征点2D坐标 - 估计的3D点坐标投影到图像上的2D坐标

用这个重投影误差的约束等式来作为观测模型，但前提是需要知道特征点的3D坐标，而实际应用中特征点的3D坐标是未知的。

EKF-SLAM的做法是将特征点加入到状态向量进行估计，但它的状态向量会随特征点的增多而变得非常大。
MSCKF的做法是根据历史相机位姿和观测来三角化计算特征点的3D坐标。这又带来了一个问题：如何确保三角化的精度呢？如果三角化误差太大，那么观测模型就会不准，最终会使得VIO精度太差。MSCKF做法是当特征点跟踪丢失后再进行三角化，特征点跟丢表示该特征的观测不会再继续增加了，这时利用所有的历史观测三角化。所以MSCKF中观测更新的时机是特征点跟丢。

2.3 MSCKF算法步骤

MSCKF算法步骤如下：

IMU积分：先利用IMU加速度和角速度对状态向量中的IMU状态进行预测，一般会处理多帧IMU观测数据。
相机状态扩增：每来一张图片后，计算当前相机状态并加入到状态向量中, 同时扩充状态协方差.
特征点三角化：然后根据历史相机状态三角化估计3D特征点
特征更新：再利用特征点对多个历史相机状态的约束，来更新状态向量。注意：这里不只修正历史相机状态，因为历史相机状态和IMU状态直接也存在关系(相机与IMU的外参)，所以也会同时修正IMU状态。
历史相机状态移除：如果相机状态个数超过N，则剔除最老或最近的相机状态以及对应的协方差.

MSCKF状态propagation和update的流程如下图所示：

请添加图片描述

图中X表示状态向量， P表示对应的协方差矩阵，红色表示当前步骤发生改变的量。

首先初始化状态向量和协方差
然后进行IMU积分，状态向量和协方差都发生改变
接着将新的相机状态加入到状态向量中，扩充协方差矩阵（新相机自身的协方差以及对 $X_{IMU}$ 的协方差）
进行观测更新，所有状态和协方差都会发生改变。（注意：第一次因为只有一个相机状态，形成不了重投影约束，所以第一次观测更新并不会做任何事情）
当相机状态个数超过限制时，删除最历史的一个相机状态及其对应的协方差项。
重复2-5。

2.4 LARVIO算法流程

除了2.3中MSCKF的操作之外LARVIO还有以下几个部分：

视觉前端：Good Feature+金字塔光流跟踪+利用ORB描述子对光流跟踪结果进行筛选
自动初始化：同时针对静止和动态的情况进行初始化。静止初始化与MSCKF_VIO相似，但可以自动选取静止的数据进行初始化，而不需要保证数据静止。动态初始化则是直接采用了VINS-MONO的方法。
传感器参数在线标定：包括相机和IMU相对外参、相机和IMU时间戳误差以及IMU内参标定。外参标定是采用了MSCKF2.0中的方法，时间戳误差标定采用了VINS-MONO的方法。IMU内参标定则参考了kalibr中提供的方法。
零速矫正：基于微惯性的行人导航的一个常规操作，其目的是利用脚落地的一瞬间的静止来抑制IMU积分结果的漂移。当载体静止时间太长时，会导致滤波器的漂移。ZUPT采用了一种closed-form的测量更新方式——在检测到静止时停止常规的测量更新，而是采用ZUPT约束进行测量更新。LARVIO采用的ZUPT约束包括三项：a.当前速度为零 b.静止的两帧间相对位置不变 c.静止的两帧间相对姿态不变

具体的算法流程如下图所示：
请添加图片描述
1.视觉前端光流跟踪：采用Good Feature+金字塔光流跟踪，经过一系列的过滤操作（imu预测、反向光流、ORB描述子过滤、F矩阵RANSAC过滤）筛选出合适的特征。

2.自动初始化：同时针对静止和动态的情况进行初始化。静止初始化与MSCKF_VIO相似，但可以自动选取静止的数据进行初始化，而不需要保证数据静止。动态初始化则是直接采用了VINS-MONO的方法。

3.INS和协方差传播：先利用IMU加速度和角速度对状态向量中的IMU状态进行预测，一般会处理多帧IMU观测数据，估计新的状态向量和对应的协方差矩阵。

4.状态增广：每来一张图片后，计算当前相机状态并加入到状态向量中, 同时扩充状态协方差矩阵。

5.测量更新：根据静态场景的检测结果，在静态场景下采用ZUPT零速矫正进行测量更新，在动态场景下基于特征的测量更新，更新MSCKF特征残差、旧的和新的增广特征残差，其中新的增广特征采用的是一维逆深度参数化的方法。

3 LARVIO算法原理

在传统的EKF-SLAM框架中，特征点的信息会加入到特征向量和协方差矩阵里，其缺点是特征点的信息会给一个初始深度和初始协方差，如果不正确的话，极容易导致后面不收敛，出现inconsistent的情况。MSCKF会通过一个滑动窗口来维护一定数量的pose，如果一个特征点被滑动窗口中几个位姿观察到的话，就会在这几个位姿间建立约束，从而进行KF的更新。如下图所示, 左边代表的是传统EKF SLAM，红色五角星是old feature，这个也是保存在状态向量中的，另外状态向量中只保存最新的相机姿态;中间这张可以表示的是keyframe-based SLAM，它会保存稀疏的关键帧和它们之间相关联的地图点; 最右边这张则可以代表MSCKF的一个基本结构，MSCKF中老的地图点和滑窗之外的相机姿态是被丢弃的，它只存了滑窗内部的相机姿态和它们共享的地图点。

请添加图片描述

3.1 视觉前端光流跟踪

VIO算法的核心在后端，不管是优化的方法还是滤波的方法，都是靠后端的紧耦合融合获得较高的VIO精度，为了节省计算量一般前端都会做得很简单，基本都是FAST、Harris等角点加LK光流跟踪，而LARVIO则是采用goodFeatureToTrack+LK光流跟踪。

3.1.1 构建金字塔图像

调用opencv中的buildOpticalFlowPyramid创建金字塔图像。

3.1.2 前后帧跟踪

对上一帧图像的特征点，在当前帧图像进行跟踪，获得当前帧图像的跟踪特征点。

1.IMU旋转估计

收集前后两帧之间所有的IMU数据，计算平均角速度，通过外参转到相机系，并根据角速度、时间和Rodrigues公式计算出相对旋转。 $\bar{w}_{imu}=\frac{1}{N} {\textstyle \sum_{j=1}^{N}}w_j$ ${w}_{cam}= ^{cam}_{imu}R\bar{w}_{imu}$ $\delta R_{cam}={w}_{cam}\delta t$
2.特征点预测

对于前一帧特征点 $p_p$ ，投影到后一帧为 $sp_c=K\cdot (\delta R\cdot d\cdot K^{-1}\cdot p_p+\delta t)$ 其中s为尺度，d为特征点的深度， $\delta t$ 为时间，这里假设d=1， $\delta t =0$ ，这样相当于将所有的特征点都归一化到深度为1的平面，因此可以直接计算Homogrphy，即： $H=K\cdot\delta R \cdot K^{-1}$ 从而可以得到投影公式简化为： $sp_c=Hp_p$ 虽然这样假设会存在误差，但这只是对特征点的初始位置的预测，提供一个先验信息，后面还会通过LK光流对位置进行refine。

3.LK光流跟踪
调用opencv中的calcOpticalFlowPyrLK进行跟踪。

3.1.3 筛选过滤

将图像区域外的跟踪点剔除
使用反向光流跟踪剔除外点
利用ORB描述子剔除距离大于58的外点
采用F矩阵进行RANSAC过滤外点

3.2 IMU自动初始化

IMU初始化的目的是：

有助于提高该紧耦合系统鲁棒性，有助于状态估计更快收敛（由于imu加速度值通常不准，对速度很敏感的应用场景，在系统融合前估计出较准的速度也很重要）
为后续预积分（用到 $g^w$ ，需要重力对齐）计算铺垫（重新预积分）

LARVIO中提供了自动的初始化方法，且同时能够针对载体静止和动态的情况。

静止初始化与MSCKF_VIO中的类似，但改进之处在于可以自动选取静止的数据进行初始化，无需像前者那样必须保证初始阶段的数据静止。静止的检测利用了基于图像的方法，但比较偏经验性，还需进一步改进。动态初始化则照搬了VINS-MONO的方法。

两种初始化进行了一个简单的逻辑封装，使得LARVIO能够在任意条件满足时立刻初始化。

3.2.1 imu静态初始化

静态初始化主要是想获取imu在起始阶段的朝向，不希望会由于起始角的偏差与地面不对齐。

1.计算陀螺仪bias：
静止时刻的陀螺仪理想测量值（角速度）应该为0，因此陀螺仪的bias即检测到静止状态下陀螺仪数据的平均值: $b_g=\frac{1}{n} \sum_{1}^{n} w_m$
2.计算加速度计bias:
静止时的加速度计bias为加速度平均值与实际重力加速度的差： $b_a=\frac{1}{n} \sum_{1}^{n} a_m-Rg$ g为世界系下的重力加速度，Ｒ为世界系到imu系的旋转矩阵，将重力矢量从世界系投影到imu坐标系下。

3.2.2 imu动态初始化

主要流程：

估计旋转外参
利用SfM确定各个pose和特征点的相对于 $c_0$ 帧的位置关系
利用相机旋转约束标定IMU角速度bias
利用IMU的平移估计重力/各bk帧速度/尺度scaler
利用gw的模长已知这个先验条件进一步优化gc0
利用gc0和gw确定世界坐标系

1.估计旋转外参

根据IMU和视觉部分的旋转关系，可以得到下面的关系式： $R_{c_{k+1}}^{b_k}=R_{b_{k+1}}^{b_k}\cdot R_c^b=R_c^b\cdot R_{c_{k+1}}^{c_k}$ $q_{b_{k+1}}^{b_k}\cdot q_c^b=q_c^b\cdot q_{c_{k+1}}^{c_k}$ $[Q_1(q_{b_{k+1}}^{b_k})-Q_2(q_{c_{k+1}}^{c_k})]\cdot q_c^b=Q_c^b\cdot q_c^b=0$ 将多个帧之间的等式关系一起构建超定方程Ax=0。对A进行svd分解，其中最小奇异值对应的奇异向量便为需要求解的 $q_c^b$ 。虽然这里IMU的旋转部分并没有标定，得到的外参数可能不太准。但由于初始化所占用的总运行时间不长，而更长生命周期的后端会持续的优化这部分的值。

2.利用SfM确定各个pose和特征点的相对于 $c_0$ 帧的位置关系

这一部分和基于图像的三维重建比较像，可以用三角化和PnP所有 $c_k$ 帧的位姿和特征点位置确定下来(特征点是伪深度)，再通过外参数 $q_c^b$ 和 $p_c^b$ ，将 $b_k$ 帧的位姿也确定下来。注意，这里把 $c_0$ 帧作为基础帧，实际上， $c_0$ 帧旋转一下使 $g_{c_0}$ 和 $g_w$ 方向一致时获得的坐标系就是larvio的世界坐标系，也就是先验。

首先我们先推导论文[1]中式(14)，所有帧的位姿 $(R^{c_0}_{c_k},p^{c_0}_{c_k})$ 表示相对于第一帧相机坐标系(·)c0。相机到IMU的外参为 $R_c^b, p_c^b)$ ，得到姿态从相机坐标系转换到IMU坐标系的关系。
$T^{c_0}_{c_k}=T^{c_0}_{b_k}\cdot T_c^b$ 将T展开成R与p有：
$\begin{bmatrix} R^{c_0}_{c_k} & sp^{c_0}_{c_k} \\ 0 & 1\end{bmatrix}=\begin{bmatrix} R^{c_0}_{b_k} & sp^{c_0}_{b_k} \\ 0 & 1\end{bmatrix}\begin{bmatrix} R_c^b & p_c^b \\ 0 & 1\end{bmatrix}=\begin{bmatrix} R^{c_0}_{b_k}R_c^b & R^{c_0}_{b_k}p_c^b+sp^{c_0}_{b_k} \\ 0 & 1\end{bmatrix}$
根据上式可得：
$R^{c_0}_{c_k}=R^{c_0}_{b_k}R_c^b\Rightarrow R^{c_0}_{b_k}=R^{c_0}_{c_k}(R_c^b)^{-1}$ $sp^{c_0}_{c_k} =R^{c_0}_{b_k}R_c^b+sp^{c_0}_{b_k}\Rightarrow sp^{c_0}_{b_k} =sp^{c_0}_{c_k}-R^{c_0}_{b_k}p_c^b$

3.利用相机旋转约束标定IMU角速度bias

求解的目标函数如下公式所示：

请添加图片描述

在完成SfM和外参数标定后， ${q_{b_{k+1}}^{c_0}}^{-1}$ 和 $q_{b_{k}}^{c_0}$ 是已知的了，而且我们假设头两个值是准的。理想状态下，这三个数乘积应该是单位四元数，很可惜，第三个值是IMU预积分得到的，而预积分里面是有bias的。所以，通过最小化这个目标函数的，可以把旋转bias标定出来！
在IMU预积分部分，也就是论文[1]中公式(12)，有 $\gamma_{b_{k+1}}^{b_k} = \hat{\gamma}_{b_{k+1}}^{b_k} \otimes \begin{bmatrix}1\\\frac{1}{2} J_{b_w}^\gamma \delta b_{w_k}\end{bmatrix}$ 代入到损失函数里，可以得到： ${q_{b_{k+1}}^{c_0}}^{-1} \otimes q_{b_{k}}^{c_0} \otimes \gamma_{b_{k+1}}^{b_k}=\begin{bmatrix}1\\0\end{bmatrix}$ 或者 $\gamma_{b_{k+1}}^{b_k}={q_{b_{k}}^{c_0}}^{-1} \otimes q_{b_{k+1}}^{c_0} \otimes \begin{bmatrix}1\\0\end{bmatrix}$ 代入bias的残差后，得到：
$\hat{\gamma}_{b_{k+1}}^{b_k} \otimes \begin{bmatrix}1\\\frac{1}{2} J_{b_w}^\gamma \delta b_{w_k}\end{bmatrix}={q_{b_{k}}^{c_0}}^{-1} \otimes q_{b_{k+1}}^{c_0} \otimes \begin{bmatrix}1\\0\end{bmatrix}$ 由于实部没有需要标定的量，所以只用考虑虚部，也就是：
$J_{b_w}^\gamma \delta b_{w_k}=2({\hat{\gamma}_{b_{k+1}}^{b_k}}^{-1} \otimes {q_{b_{k}}^{c_0}}^{-1} \otimes q_{b_{k+1}}^{c_0})_{vec}$ 两侧再乘以 ${J_{b_w}^\gamma}^{T}$ ，可以构造出Ax=B的形式，再采用LDLT分解，就可以求出状态量 $\delta b_w$ ：
${J_{b_w}^\gamma}^{T}J_{b_w}^\gamma \delta b_{w_k}=2{J_{b_gw}^\gamma}^{T}({\hat{\gamma}_{b_{k+1}}^{b_k}}^{-1} \otimes {q_{b_{k}}^{c_0}}^{-1} \otimes q_{b_{k+1}}^{c_0})_{vec}$

4.利用IMU的平移估计重力/各 $b_k$ 帧速度/尺度scale

首先要明确需要优化的状态量是什么，是各帧在 $b_k$ 坐标系下的速度， $c_0$ 帧下的g和SfM的尺度scale：
请添加图片描述

在IMU预积分部分，已经有如下的公式：
请添加图片描述

但是 $w$ 坐标系不知道，只知道 $c_0$ 坐标系，所以需要把上面的公式转到 $c_0$ 坐标系上：
$\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}(s p_{b_{k+1}}^{c_{0}}-s p_{b_{k}}^{c_{0}}+\frac{1}{2}g^{c_{0}}\Delta t_{k}^{2}-R_{b_{k}}^{c_{0}}v_{b_{k}}^{b_{k}}\Delta t_{k})$ $\beta_{b k+1}^{b_{k}}=R_{c0}^{b_{k}}(R_{b_{k+1}}^{c_{0}}v_{b_{k+1}}^{b_{k+1}}+g^{c_{0}}\Delta t_{k}-R_{b_{k}}^{c_{0}}v_{b_{k}}^{b_{k}})$

上式中，等号左边减去等号右边就是残差，理想状态下，这个残差是0，那么带入上式得：
$\begin{array}{c}\delta \alpha_{b_{k+1}}^{b_{k}}=\alpha_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}}\left(s p_{b_{k+1}}^{c_{0}}-s p_{b_{k}}^{c_{0}}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}} \Delta t_{k}\right) \\ =\alpha_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}}\left(s p_{c_{k+1}}^{c_{0}}-R_{b_{k+1}}^{c_{0}} p_{c}^{b}-\left(s p_{c_{k}}^{c_{0}}-R_{b_{k}}^{c_{0}} p_{c}^{b}\right)+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}} \Delta t_{k}\right) \\ =\alpha_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}} s\left(p_{c_{k+1}}^{c_{0}}-p_{c_{k}}^{c_{0}}\right)+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}+v_{b_{k}}^{b_{k}} \Delta t_{k}-\frac{1}{2} R_{c_{0}}^{b_{k}} g^{c_{0}} \Delta t_{k}^{2}=0_{3 \times 1}\end{array}$
把这个 XX = 0的等式也转为 A x = b这种线性方程组的形式，如下式：
$R_{c_{0}}^{b_{k}}\left(p_{c k+1}^{c_{0}}-p_{c_{k}}^{c_{0}}\right) s-v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2} g^{c_{0}}=\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}$
或者矩阵的形式：
$\begin{bmatrix} -I\Delta t_k & 0 & \frac{1}{2}R_{c_{0}}^{b_{k}}\Delta t_k^2 & R_{c_{0}}^{b_{k}}(p_{c_{k+1}}^{c_{0}} - p_{c_{k}}^{c_{0}}) \end{bmatrix} \begin{bmatrix} v_{b_{k}}^{b_{k}}\\ v_{b_{k+1}}^{b_{k+1}}\\ g^{c_0}\\ s\end{bmatrix} =\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}$
对于beta而言，也是类似的:
$\delta \beta_{b_{k+1}}^{b_{k}}=\beta_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}}\left(R_{b_{k+1}}^{c_{0}} v_{b_{k+1}}^{b_{k+1}}+g^{c_{0}} \Delta t_{k}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}}\right)=0_{3 \times 1}$ $R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} v_{b_{k+1}}^{b_{k+1}}-R_{c_{0}}^{b_{k}} \Delta t_{k} g^{c_{0}}+R_{c_{0}}^{b_{k}} R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}}=\beta_{b_{k+1}}^{b_{k}} \\$ ${\left[\begin{array}{llll}-I & R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} & R_{c_{0}}^{b_{k}} \Delta t_{k} & 0\end{array}\right]\left[\begin{array}{c}v_{b_{k}}^{b_{k}} \\ v_{b k+1}^{b_{k+1}} \\ g^{c_{0}} \\ s\end{array}\right]=\beta_{b_{k+1}}^{b_{k}}}$

那么，把这两个矩阵合体，就能得到论文[1]中的等式(18)：
$\left[\begin{array}{cccc}-I \Delta t_{k} & 0 & \frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2} & R_{c_{0}}^{b_{k}}\left(p_{c_{k+1}}^{c_{0}}-p_{c_{k}}^{c_{0}}\right) \\ -I & R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} & R_{c_{0}}^{b_{k}} \Delta t_{k} & 0\end{array}\right]\left[\begin{array}{c}v_{b_{k}}^{b_{k}} \\ v_{b_{k+1}}^{b_{k+1}} \\ g^{c_{0}} \\ s\end{array}\right]=\left[\begin{array}{c}\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b} \\ \beta_{b_{k+1}}^{b_{k}}\end{array}\right]$
同样采用LDLT分解，就能求出状态量 $\chi _I^{10×1}$ ：
$H^TH \chi _I^{10×1}=H^Tb$

5.修正重力矢量 $g^{c_0}$

在上一步的操作中得到的g向量是三维度的，但是优化过程中并没有加入其自身的模长为9.81的约束，根据论文[1]中给的图，如下图所示，其加入约束 $g_{c_0}=|g|\cdot\hat{g}+w_{1}b_{1}+w_{2}b_{2}$ ，通过上面求解的 $\hat{g}$ 作为观测值，对应代码中 $g_0$ 。
请添加图片描述

在这里，采用球面坐标进行参数化，也就是用g的模长作为半径画一个半球，上图蓝色线对应的是 $g_{c_0}$ 的测量值的方向(也就是优化前的方向)，在这个交点上找到一个切平面，用 $g_{c_0}$ ， $b_1$ ， $b_2$ 构造一个坐标系，那么在轴 $b_1$ 和 $b_2$ 上坐标值 $w_1和w_2$ 就是我们需要求的量。只需要将此模值约束带入论文[1]公式(18)即可得到一个新的等式如下，最后同样的是求解Ax=b的过程。其中假设一个轴为(0,0,1)，然后采用的史密斯正交化求解出 $b_1$ ，之后由 $g_{c_0}$ 的测量值的方向与 $b_1$ 作叉乘得到 $b_2$ 。
$\left.\left[\begin{array}{ccc}-I\Delta t_k&0&\frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2\vec{b}&R_{c_b}^{b_k}(p_{c_{k+1}}^{c_0}-p_{c_k}^{c_0})\\-I&R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}&R_{c_0}^{b_k}\Delta t_k\vec{b}&0\end{array}\right.\right]\left[\begin{array}{c}v_{b_k}^{b_k}\\v_{b_{k+1}}^{b_{k+1}}\\w\\s\end{array}\right]=\left[\begin{array}{c}\alpha_{b_{k+1}}^{b_k}+R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b-p_c^b-\frac{1}{2}R_{c_0}^{b_k}\Delta t_k^{b_k}||g||\vec{\hat{g}}\\\beta_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}\Delta t_k||g||\vec{\hat{g}}\end{array}\right]$
同样使用LDLT分解： $H^TH\mathcal{X}_I^{9\times1}=H^Tb$

6 重力方向对齐（将所有变量调整至惯性世界坐标系）

这部分主要是计算相机系到惯性系的旋转矩阵 $q_{c_0}^w$ ，将 $C_0$ 系下的重力向量 $g^{}$ 旋转至惯性坐标系中的Z轴方向，并把所有变量调整至惯性坐标系。

(1)找到 $c_0$ 到 $w$ 系的旋转矩阵 $R_{c_0}^w = exp([θu])$
$\mathbf{u}=\frac{\hat{\mathbf{g}}^{c_0}\times\hat{\mathbf{g}}^w}{\|\hat{\mathbf{g}}^{c_0}\times\hat{\mathbf{g}}^w\|},\theta=atan2(\|\hat{\mathbf{g}}^{c_0}\times\hat{\mathbf{g}}^w\|,\hat{\mathbf{g}}^{c_0}\cdot\hat{\mathbf{g}}^w)$ 由于 $g^{c_0}$ 已经求出来了，而 $g^w$ 是一个已知的量，因此它们之间的夹角 $θ$ 可以由上式求得，然后用 $g^{c_0}$ 和 $g^w$ 作叉乘得到一个旋转轴u；最后把 $c_0$ 坐标系绕着转轴旋转一个θ，就能找到 $c_0$ 到 $w$ 系的对齐关系，也就是 $R_{c_0}^w = exp([θu])$ 。