VINS分析（上）

VINS分析（上）

28 November, 2017
Ling Bao

---

1 VINS源码分析

1.1 配置文件

  包括:共有参数(ROS相关)、相机矫正参数、 IMU和相机之间的外部参数、特征跟踪参数、 优化参数、IMU参数、回环控制参数。

1.2 VINS系统分析

[1] 港科VINS-Mono系统学习（1）
[2] 港科VINS-Mono系统学习（2） 预处理
[3] 港科VINS-Mono系统学习（3） 初始化
[4] 港科VINS-Mono系统学习（4） 里程计
[5] 港科VINS-Mono系统学习（5） 闭环检测
[6] VINS（一）简介与代码结构
[7] VINS（二）Feature Detection and Tracking
[8] imu标定 imu_tk
[9] VINS（三）IMU预积分
[10] VINS（四）初始化与相机IMU外参标定
[11] VINS（五）非线性优化与在线标定调整
[12] VINS（六）边缘化
[13] VINS（七）estimator_node 数据对齐 imu预积分 vision
[14] VINS（八）初始化
[15] VINS-Mono代码分析与总结(一) IMU预积分
[16] IMU误差模型和校准

---

2 论文分析

2.1 总体综述

  第一部分 传感器测量数据处理: 实现每帧视频的Harris角点(特征点)， 并利用KLT光流法实现特征点跟踪。
  
  第二部分 初始化: 包括两个核心(纯视觉SFM、视惯对齐)； 初始化的参数包括(Pose, Velocity, Gravity Vector, Gyroscope Bias, 3D Feature Position)。
  
  第三部分 后端Local BA: MU误差+重投影误差+先验误差的多目标非线性优化调整, 关键概念(滑动窗口、关键帧、 ceres 非线性优化)，重定位。
  
  第四部分 回环检测与位姿图优化: fast特征提取+BRIEF描述子、利用BoW的闭环检测，基础矩阵+RANSAC的位姿估计、4自由度Psoe-Graph优化。
  

---

2.2 符号定义

$(*)^{w}$ – word frame, gravity vector is along with z axis;

$(*)^{b}$ – body frame is aligned with IMU frame;

$(*)^{c}$ – camera frame.

$R$ – 旋转矩阵, 用于与向量相乘

$q$ – 四元数, 用于状态描述

$q_{w}^{b}$ – Rotation from body frame to world frame

$p_{w}^{b}$ – Translation from body frame to world frame

$P_{b_k}^{b_{K+1}}$ – Covariance Matrix corresponding to $p_{w}^{b}$

$b_{k}$ – body frame while taking the kth image.

$c_{k}$ – camera frame while taking the kth image.

$\bigotimes$ – Two quaternion multiplication operation.

$g^{w}=[0, 0, g]'$ – Gravity vector in the world frame.

$\hat w_t ,\hat a_t$ – body frame IMU原始测量值，角速度和加速度.

$b_a, b_w$ – body frame受加速度偏差、陀螺仪偏差及噪音的影响值.

$n_a, n_w$ – body frame受加速度偏差、陀螺仪偏差及噪音的影响值.

---

2.3 测量处理

  功能概要：新帧特Harris征点检测与连续帧间特征点KLT光流跟踪；IMU连续帧间预积分，同时预积分受bias与noise影响，因此在考虑bias条件下实现预积分并进行优化（对于低成本IMU）。
  
  视觉处理前端：（1）每个视频帧的特征点最小保持在100～300个，同时通过设置特征点之间的最小距离使得特征点分布成为均匀分布；将特征点归一化，并利用RANSAC进行Outlier点去除。（2）关键帧选择，两个准则(平均视差–利用IMU传递结果对旋转矩阵进行补偿、跟踪质量–跟踪的特征点数量)。
  
   IMU预积分：改进：增加了bias纠正以及动态连续时间内的noise传递；除此之外，IMU预积分的结果将被用于初始化程序的校正初始化状态。
  
   考虑时间点$k, k+1$，对应的body frame是$b_k, b_{k + 1}$，时间间隔为$\Delta{t_k}$，那么有：

$$p_{k+1}^{w}=p_{k}^{w}+v_{b_k}^{w}\Delta{t_k}+\iint_{t\in[k, k+1]}(R_t^{w}(\hat a_t-b_{a_t}))dt^2 \tag{1}$$
$$v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{t\in[k, k+1]}(R_t^{w}(\hat a_t-b_{a_t}))dt \tag{2}$$
$$q_{b_{k+1}^{w}}=q_{b_{k}^{w}} \bigotimes \int_{t \in [k, k+1]} \frac{1}{2} \Omega (\hat w_t-b_{w_t})q_t^{b_k}dt \tag{3}$$
$$\Omega (w)= \left[ \begin{matrix} -[w]_{\times} & w \\ -w^{T} & 0 \\ \end{matrix} \right] \tag{4}$$
$$[w]_{\times}= \left[ \begin{matrix} -0 & -w_z & w_y \\ -w_z & 0 & -w_x \\ -w_y & w_x & 0 \\ \end{matrix} \right] \tag{5}$$
   从公式 $(1)～(5)$可以看出IMU状态传递需要用到 $b_k$的 旋转矩阵、位置以及速度信息。
  
   在IMU传递过程中，reference frame变为局部body frame，采用与线性加速度 $\hat a$和角速度 $\hat w$相关的部分：
$$R_w^{b_k}p_{b_{k+1}^{w}}=R_w^{b_k}(p_{b_k}^{w}+v_{b_k}^w \Delta t_k-\frac{1}{2}g^{w} \Delta t_k^{2})+\alpha_{b_{k+1}}^{b_k} \tag{6}$$
$$R_w^{b_k}v_{b_{k+1}}^w=R_w^{b_k}(v_{b_k}^w-g^{w} \Delta t_k)+\beta_{b_{k+1}}^{b_k} \tag{7}$$
$$q_w^{b_k} \bigotimes q_{b_{k+1}}^{w}= \gamma_{b_{k+1}}^{b_k} \tag{8}$$
   其中，
$$\alpha_{b_{k+1}}^{b_k}= \iint_{t \in [k, k+1]}R_t^{b_k}(\hat a_t-b_{a_t})dt^2 \tag{9}$$
$$\beta_{b_{k+1}}^{b_k}= \int_{t \in [k, k+1]}R_t^{b_k}(\hat a_t-b_{a_t})dt \tag{10}$$
$$\gamma_{b_{k+1}}^{b_k}= \int_{t \in [k, k+1]} \frac{1}{2} \Omega(\hat w_t-b_{w_t}) \gamma_{b_t}^{b_k}dt \tag{11}$$
   $\gamma_{b_k}^{b_k}$是开始时单位四元数；从 $(9)～(11)$可以由 $b_{k}$作为基准帧的IMU测量获得； $\alpha_{b_{k+1}}^{b_k}$, $\beta_{b_{k+1}}^{b_k}$, $\gamma_{b_{k+1}}^{b_k}$仅与 $b_k$和 $b_{k+1}$间的偏差有关，假设初始时刻 $b_a, b_w$为0；当所估计的偏差值改变比较小时，调整 $(9)～(11)$（有关偏差的一阶近似），否则进行重传递 $(6)～(8)$；该方法可以节省大量时间。
  
   针对离散情况，各种数值积分方法可以使用，如 Euler, Mid-point, RK4积分等；本论文中使用 Euler积分进行说明，而程序实现中使用 Mid-point积分。
  
   开始时 $\alpha_{b_{k+1}}^{b_k}$, $\beta_{b_{k+1}}^{b_k}$为 0, $\gamma_{b_{k+1}}^{b_k}$为单位四元数，代表单位旋转矩阵；此时的 $\alpha, \beta, \gamma$为：
$$\hat \alpha_{i+1}^{b_{k}}=\hat \alpha_{i}^{b_{k}}+\hat \beta_{i}^{b_{k}}\delta t+\frac{1}{2}R(\hat \gamma_{i}^{b_{k}})(\hat a_i-b_{a_i}) \delta t^2 \tag{12}$$
$$\hat \beta_{i+1}^{b_{k}}=\hat \beta_{i}^{b_{k}}+R(\hat \gamma_{i}^{b_{k}})(\hat a_i-b_{a_i}) \delta t \tag{13}$$
$$\hat \gamma_{i+1}^{b_{k}}=\hat \gamma_{i}^{b_{k}} \bigotimes \left[ \begin{matrix} 1 \\ \frac{1}{2}(\hat w_i-b_{w_i}) \delta t \end{matrix} \right] \tag{14}$$
   其中， $i, i+1$对应与 $[k, k+1]$的两个IMU测量； $\delta t$是 $i, i+1$对应的时间间隔。
  
   协方差传递处理，假设加速度计、陀螺仪的测量噪音满足 Gaussian white noise，即： $n_a～\cal N(0, \sigma_a^2)$, $n_w～\cal N(0, \sigma_w^2)$；加速度、陀螺仪偏差满足随机游走分布，该分布的相邻项的差满足 Gaussian white noise，即： $n_{b_a}～\cal N(0, \sigma_{b_a}^2)$， $n_{b_w}～\cal N(0, \sigma_{b_w}^2)$；
$$\dot b_{a_t}=n_{b_a}, \dot b_{w_t}=n_{b_w} \tag{15}$$
   $\gamma_{t}^{b_k}$受扰动误差项影响，定义如下：
$$\gamma_{t}^{b_k} \approx \hat \gamma_{t}^{b_{k}} \bigotimes \left[ \begin{matrix} 1 \\ \frac{1}{2} \delta \theta_t^{b_k} \end{matrix} \right] \tag{16}$$
   从 $(9)～(15)$可以推导出误差项的连续时间线性动态，如下：
$$\left[ \begin{matrix} \delta \dot \alpha_{t}^{b_k} \\ \delta \dot \beta_{t}^{b_k} \\ \delta \dot \theta_{t}^{b_k} \\ \delta \dot b_{a_t} \\ \delta \dot b_{w_t} \\ \end{matrix} \right]= \left[ \begin{matrix} 0 & \bf I & 0 & 0 & 0 \\ 0 & \bf 0 & -R_t^{b_k}[\hat a_t-b_{a_t}]_{\times} & -R_t^{b_k} & 0 \\ 0 & \bf 0 & -[\hat w_t-b_{w_t}]_{\times} & 0 & -\bf I \\ 0 & \bf 0 & 0 & 0 & 0 \\ 0 & \bf 0 & 0 & 0 & 0 \\ \end{matrix} \right] \left[ \begin{matrix} \delta \alpha_{t}^{b_k} \\ \delta \beta_{t}^{b_k} \\ \delta \theta_{t}^{b_k} \\ \delta b_{a_t} \\ \delta b_{w_t} \\ \end{matrix} \right]+$$
$$\left[ \begin{matrix} \bf 0 & 0 & 0 & 0 \\ \bf -R_t^{b_k} & 0 & 0 & 0 \\ \bf 0 & -\bf I & 0 & 0 \\ \bf 0 & 0 & \bf I & 0 \\ \bf 0 & 0 & 0 & -\bf I \\ \end{matrix} \right] \left[ \begin{matrix} \bf n_{a} \\ \bf n_{w} \\ \bf n_{b_a} \\ \bf n_{b_w} \\ \end{matrix} \right]= \bf F_t \delta \bf z_{t}^{b_k}+ \bf G_{t} \bf {n_t} \tag{17}$$
   $[*]\times$代表反对称操作； $P_{b_{k+1}^{b_k}}$可以利用一阶离散时间协方差更新进行计算，初始 $P_{b_{k}^{b_k}}=0$，如下所示：
$$P_{b_{b+\delat t}^{b_k}}=(I+F_{t} \delta t) P_t^{b_k}(I+F_{t} \delta t)^T+(G_t \delta t)Q(G_t \delta t)^T, t \in [k, k+1] \tag{18}$$
   其中 $\delta t$是连续两帧IMU测量的时间间隔， $Q$是噪音对角协方差矩阵$(\sigma_{a}^2, \sigma_{w}^2, \sigma_{b_a}^2, \sigma_{b_w}^2)$。
  
   与此同时， $z_{k+1}^{b_k}$的一阶Jacobian矩阵 $J_{b_{k+1}}$也可以迭代计算，初始Jacobian矩阵是 $J_{b_{k}}=I$，如下所示：
$$J_{t+\delta t}=(I+F_t \delta t)J_{t}, t \in [k, k+1] \tag{19}$$
   那么 $\alpha_{b_{k+1}}^{b_k}$, $\beta_{b_{k+1}}^{b_k}$, $\gamma_{b_{k+1}}^{b_k}$的偏差一阶近似如下：
$$\alpha_{b_{k+1}}^{b_k} \approx \hat \alpha_{b_{k+1}}^{b_k}+J_{b_a}^ \alpha \delta b_{b_{a_{k}}}+J_{b_w}^ \alpha \delta b_{w_k} \tag {20}$$
$$\beta_{b_{k+1}}^{b_k} \approx \hat \beta_{b_{k+1}}^{b_k}+J_{b_a}^ \beta \delta b_{b_{a_{k}}}+J_{b_w}^ \beta \delta b_{w_k} \tag{21}$$
$$\gamma_{b_{k+1}}^{b_k} \approx \hat \gamma_{b_{k+1}}^{b_k} \bigotimes \left[ \begin{matrix} 1 \\ \frac{1}{2} J_{b_w}^ \beta \delta b_{w_k} \end{matrix} \right] \tag{22}$$
   $J_{b_a}^ \alpha$是 $J_{b_{k+1}}$中对应于 $\frac{\delta \alpha_{b_{k+1}}^{b_k}}{\delta b_{a_k}}$的子块； $J_{b_w}^ \alpha，J_{b_a}^ \beta，J_{b_w}^ \beta，J_{b_w}^ \beta与此相同；\gamma_{b_{k+1}}^{b_k}$仅受 $b_{w_k}$影响，当偏差估计发生轻微改变时，使用 $(22)$进行预积分矫正来近似代替重传递。