VINS-Mono论文知识点精炼(翻译)

论文地址: VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

摘要

• VINS-Mono:一种鲁棒的和多功能的单目视觉惯性状态估计器
• 鲁棒的估计器初始化
• 紧耦合非线性优化方法融合IMU测量值和特征观测值，获取高精度视觉惯性里程计
• 使用低功耗回环检测模块
• 四自由度全局位姿图优化
• 地图可保存、加载和拼接

关键字: 单目视觉惯性系统、状态估计、传感器融合、同时定位建图

引言

• 单目
  优点: 尺寸小、低功耗、安装简易．
  缺点: 无法获取尺度信息
• 单目+IMU
  优点: 获取尺度信息、部分姿态信息(roll, pitch)、提升跟踪性能(当光照变换、纹理缺失或运动模糊时)
  问题:(1). 需要严谨的初始化: 视觉惯性系统是一个高度非线性系统，初始化很具挑战性；
          (2). 启动有限制: 大多数视觉惯性系统需要在指定的地方启动，并且需要缓慢小心移动；
          (3). 漂移: 已经存在基于回环检测、重定位和全局优化方法消除漂移；
          (4). 地图的重用: 需要有能够对地图进行保存和加载的方法．
• VINS-Mono特点
  (1). 鲁棒的初始化程序能够从未知的初始状态引导系统；
  (2). 紧耦合的基于优化的单目VIO，带有相机IMU外参标定和IMU偏置矫正；
  (3). 在线重定位和四自由度全局位姿图优化；
  (4). 位姿图重用: 保存、加载和融合多个局部位姿图．

概述

从上面的框架图中可以获得VINS-Mono的组成如下:

• 测量的预处理
  (1) 图像特征提取和跟踪
  (2) 预积分两图像帧之间的IMU测量值
• 初始化
  提供引导基于非线性优化VIO系统所需的值，包括位姿、速度、重力向量、陀螺仪偏置和三维特征位置
• 基于优化的VIO
• 重定位
• 位姿图优化
  (1) 利用经过几何一致性验证后的重定位结果，执行全局位姿图优化，消除漂移
  (2) 位姿图的重用

测量的预处理

A.视觉预处理

• 特征跟踪: 使用KLT稀疏光流算法
• 特征检测: (1) 检测新特征，维护每张图像最小特征数量(100~300)
                  (2) 设定相邻特征之间的最小距离，保证特征分布均匀
• 离群特征丢弃: 使用基于基础矩阵的RANSAC方法
• 特征生成: 去畸变，然后投影到单位球面上
• 关键帧生成: 满足足够视差或跟踪质量太差任一条件即可

B.IMU预积分

• IMU噪声和偏置
  陀螺仪和加速度计测量值:
  $$\begin{array}{rl}& {\hat{\mathbf{a}}}_t={\mathbf{a}}_t+{\mathbf{b}}_{a_t}+{\mathbf{R}}_w^t{\mathbf{g}}^w+{\mathbf{n}}_a\\ & {\hat{\mathbf{\omega }}}_t={\mathbf{\omega }}_t+{\mathbf{b}}_{w_t}+{\mathbf{n}}_w\end{array}\text{\hspace{1em}(1)}$$
  陀螺仪和加速度测量噪声为高斯白噪声: ${\mathbf{n}}_a\sim \mathbf{N}(0,{\mathbf{\sigma }}_a^2),{\mathbf{n}}_w\sim (0,{\mathbf{\sigma }}_w^2)$
  陀螺仪和加速度计被建模成随机游走模型，满足:${\dot{\mathbf{b}}}_{a_t}={\mathbf{n}}_{b_a},{\dot{\mathbf{b}}}_{w_t}={\mathbf{n}}_{b_w}$，其中 ${\mathbf{n}}_{b_a}\sim \mathbf{N}(0,{\mathbf{\sigma }}_{b_a}^2),{\mathbf{n}}_{b_w}\sim (0,{\mathbf{\sigma }}_{b_w}^2)$.
• 预积分(PVQ增量)
  $$\begin{array}{rl}& {\mathbf{\alpha }}_{b_{k+1}}^{b_k}={\iint }_{t\in [t_k,t_{k+1}]}{\mathbf{R}}_t^{b_k}({\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t})d{t}^2\\ & {\mathbf{\beta }}_{b_{k+1}}^{b_k}={\int }_{t\in [t_k,t_{k+1}]}{\mathbf{R}}_t^{b_k}({\hat{\mathbf{a}}}_t-{\mathbf{b}}_{a_t})dt\\ & {\mathbf{\gamma }}_{b_{k+1}^{b_k}}={\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\mathbf{\Omega }({\hat{\mathbf{\omega }}}_t-{\mathbf{b}}_{w_t}){\mathbf{\gamma }}_t^{b_k}dt\end{array}\text{\hspace{1em}(3)}$$
  其中:
  $$\mathbf{\Omega }(\mathbf{\omega })=\left[\begin{array}{cc}-{\lfloor \mathbf{\omega }\rfloor }_{\times }& \mathbf{\omega }\\ -{\mathbf{\omega }}^T& 0\end{array}\right],{\lfloor \mathbf{\omega }\rfloor }_{\times }=\left[\begin{array}{ccc}0& -{\mathbf{\omega }}_z& {\mathbf{\omega }}_y\\ {\mathbf{\omega }}_z& 0& -{\mathbf{\omega }}_x\\ -{\mathbf{\omega }}_y& {\mathbf{\omega }}_x& 0\end{array}\right]\text{\hspace{1em}(4)}$$
  从预积分增量的表达式中可以看出预积分增量是用$b_k$作为参考坐标系，因此其它时刻的状态发生改变时不需要对PVQ增量进行重新积分．但表达式显示该积分增量与陀螺仪和加速度计偏置有关，也就意味着如果在优化过程中出现陀螺仪和加速度计的偏置发生改变，需要在优化的过程中不断进行上述积分过程，这是个巨大的硬件资源消耗，因此下面提出了一种近似方法．
• 偏置矫正
  如果偏置${\mathbf{b}}_{a_t},{\mathbf{b}}_{w_t}$发生小的改变，则按下式进行矫正:
  $$\begin{array}{rl}& {\mathbf{\alpha }}_{b_{k+1}}^{b_k}={\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\alpha }\delta {\mathbf{b}}_{a_k}+{\mathbf{J}}_{b_w}^{\alpha }\delta {\mathbf{b}}_w\\ & {\mathbf{\beta }}_{b_{k+1}}^{b_k}={\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k}+{\mathbf{J}}_{b_a}^{\beta }\delta {\mathbf{b}}_{a_k}+{\mathbf{J}}_{b_w}^{\beta }\delta {\mathbf{b}}_w\\ & {\mathbf{\gamma }}_{b_{k+1}^{b_k}}={\hat{\mathbf{\gamma }}}_{b_{k+1}^{b_k}}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\gamma }\delta {\mathbf{b}}_w\end{array}\right]\end{array}\text{\hspace{1em}(5)}$$
  否则的话，由于偏置有明显的变化，我们需要重新利用的新的偏置对PVQ增量预积分公式进行重新积分．该策略能够有效的避免了在优化过程中频繁的进行积分，降低了对硬件资源的使用．

初始化

A.滑窗内纯视觉SfM

我们为了限制计算复杂性，我们只维护了滑窗内的数据．首先我们检查最新帧与先前所有帧之间的相关性，如果在最新帧和滑窗内其它的任意一帧之间存在稳定的特征跟踪(超过30个特征点)并且有足够的视差(超过20个像素)，则使用5点法恢复这两帧之间的相对旋转和缩放过的移动．然后，我们随机设置一个尺度因子，三角化这两帧之间的所有特征点．基于这些三角化过的特征点，使用PnP方法估计窗口内所有其它帧的位姿．最后，使用一个全局BA方法最小化所有观察到的特征的总的重投影误差．设置第一个相机帧坐标系$(\cdot {)}^{c_0}$作为SfM的参考坐标系，则所有其它帧的位姿定义为$({\bar{\mathbf{p}}}_{c_k}^{c_0},{\mathbf{q}}_{c_k}^{c_0})$．给定相机与IMU之间的外参$({\mathbf{p}}_c^b,{\mathbf{q}}_c^b)$，我们可以将相机位姿从相机坐标系转换到基座标系(IMU)下:
$$\begin{array}{rl}{\mathbf{q}}_{b_k}^{c_0}& ={\mathbf{q}}_{c_k}^{c_0}\otimes ({\mathbf{q}}_c^b{)}^{-1}\\ s{\mathbf{p}}_{b_k}^{c_0}& =s{\mathbf{p}}_{c_k}^{c_0}-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{p}}_c^b\end{array}\text{\hspace{1em}(6)}$$
式中$s$为尺度因子(后面会计算)，${\mathbf{R}}_{b_k}^{c_0}$为${\mathbf{q}}_{b_k}^{c_0}$转换成的旋转矩阵．

B.视觉惯性对齐

思路: 对齐视觉SfM结构和IMU预积分结果．

• 陀螺仪偏置标定
  通过SfM可获得第k帧和第k+1帧姿态信息: ${\mathbf{q}}_{b_k}^{c_0},{\mathbf{q}}_{b_{k+1}}^{c_0}$
  通过预积分可获第k帧和第k+1帧之间姿态信息的变化量: ${\mathbf{\gamma }}_{b_{k+1}}^{b_k}$
  于是可通过上述信息建立相邻两帧之间的约束，构建残差方程:
  $$\begin{array}{rl}& \underset{\delta b_w}{\min }\sum _{k\in B}{\Vert {{\mathbf{q}}_{b_{k+1}}^{c_0}}^{-1}\otimes {\mathbf{q}}_{b_k}^{c_0}\otimes {\mathbf{\gamma }}_{b_{k+1}}^{b_k}\Vert }^2\\ & {\mathbf{\gamma }}_{b_{k+1}}^{b_k}\approx {\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\gamma }\delta {\mathbf{b}}_w\end{array}\right]\end{array}\text{\hspace{1em}(7)}$$
  式中$B$为滑动窗口内的所有帧索引集合．通过最小化公式7，即可获得陀螺仪偏置的增量，即实现陀螺仪偏置标定．在标定完陀螺仪偏置之后，需要重新利用新的偏置对IMU测量值进行预积分．
• 速度、重力向量和尺度因子的初始化
  待初始化变量: ${\chi }_I=[{\mathbf{v}}_{b_0}^{b_0},{\mathbf{v}}_{b_1}^{b_1},...,{\mathbf{v}}_{b_n}^{b_n},{\mathbf{g}}^{c_0},s]$
  其中${\mathbf{v}}_{b_k}^{b_k}$是采集第$k$张图像时基体坐标系下的速度，${\mathbf{g}}^{c_0}$是在$c_0$坐标系下的重力向量，$s$是单目SfM无法获取的尺度因子．
  考虑滑动窗口内的相邻两帧之间的关系，可建立如下PV增量和两帧状态的关系:
  $$\begin{array}{rl}& {\mathbf{\alpha }}_{b_{k+1}}^{b_k}={\mathbf{R}}_{c_0}^{b_k}(s({\bar{\mathbf{p}}}_{b_{k+1}}^{c_0}-{\bar{\mathbf{p}}}_{b_k}^{c_0})+\frac{1}{2}{\mathbf{g}}^{c_0}\Delta t_k^2-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{v}}_{b_k}^{b_k}\Delta t_k)\\ & {\mathbf{\beta }}_{b_{k+1}}^{b_k}={\mathbf{R}}_{c_0}^{b_k}({\mathbf{R}}_{b_{k+1}}^{c_0}{\mathbf{v}}_{b_{k+1}}^{b_{k+1}}+{\mathbf{g}}^{c_0}\Delta t-{\mathbf{R}}_{b_k}^{c_0}{\mathbf{v}}_{b_k}^{b_k})\end{array}\text{\hspace{1em}(9)}$$
  将公式6带入9中，然后建立如下线性测量模型:
  $${\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}-{\mathbf{p}}_c^b+{\mathbf{R}}_{c_0}^{b_k}{\mathbf{R}}_{b_{k+1}}^{c_0}{\mathbf{p}}_c^b\\ {\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k}\end{array}\right]={\mathbf{H}}_{b_{k+1}}^{b_k}{\chi }_I+{\mathbf{n}}_{b_{k+1}}^{b_k}\text{\hspace{1em}(10)}$$
  其中:
  $${\mathbf{H}}_{b_{k+1}}^{b_k}=\left[\begin{array}{cccc}-\mathbf{I}\Delta t& 0& \frac{1}{2}{\mathbf{R}}_{c_0}^{b_k}\Delta t_k^2& {\mathbf{R}}_{c_0}^{b_k}({\bar{\mathbf{p}}}_{c_{k+1}}^{c_0}-{\bar{\mathbf{p}}}_{c_k}^{c_0})\\ -\mathbf{I}& {\mathbf{R}}_{c_0}^{b_k}{\mathbf{R}}_{b_{k+1}}^{c_0}& {\mathbf{R}}_{c_0}^{b_k}\Delta t_k& 0\end{array}\right]\text{\hspace{1em}(11)}$$
  式中${\mathbf{R}}_{b_k}^{c_0},{\mathbf{R}}_{b_{k+1}}^{c_0},{\bar{\mathbf{p}}}_{c_k}^{c_0},{\bar{\mathbf{p}}}_{c_{k+1}}^{c_0}$可通过SfM过程获得，$\Delta t$是相邻两帧之间的间隔．因此可通过构建如下最小二乘问题:
  $$\underset{{\chi }_I}{\min }\sum {\Vert {\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}-{\mathbf{H}}_{b_{k+1}}^{b_k}{\chi }_I\Vert }^2\text{\hspace{1em}(12)}$$
  来优化求解出窗口中每一帧的速度、重力向量和尺度因子．
• 重力细化
  大多数情况下重力向量的大小是已知的，因此重力向量只剩下2个自由度了．本文将重力向量按下图进行定义，$\mathbf{g}=g(\overrightarrow{\hat{\mathbf{g}}}+\delta \mathbf{g}),\delta \mathbf{g}=w_1{\mathbf{b}}_1+w_2{\mathbf{b}}_2$，其中$g$是已知的重力大小，$\vec{g}$是代表重力方向的单位向量，${\mathbf{b}}_1,{\mathbf{b}}_2$是重力向量切平面上的两个互相垂直的基向量．可看出该定义保证了重力向量在大小已知的情况下只有两个自由度．
  

我们将新的重力向量$\mathbf{g}$带入公司9,然后重新进行公式12的优化求解，即可对重力向量进行细化．

紧耦合单目VIO

在初始化完成之后，我们进行基于滑窗的紧耦合单目VIO算法获取高精度和鲁棒性的状态估计．

A. 公式

• 滑窗内完整状态向量定义
  $$\begin{array}{rl}\mathbf{\chi }& =[{\mathbf{x}}_0,{\mathbf{x}}_1,...{\mathbf{x}}_n,{\mathbf{x}}_c^b,{\lambda }_0,{\lambda }_1,...{\lambda }_m]\\ {\mathbf{x}}_k& =[{\mathbf{p}}_{b_k}^w,{\mathbf{v}}_{b_k}^w,{\mathbf{q}}_{b_k}^w,{\mathbf{b}}_a,{\mathbf{b}}_{\mathbf{g}}],k\in [0,n]\\ {\mathbf{x}}_c^b& =[{\mathbf{p}}_c^b,{\mathbf{q}}_c^b]\end{array}\text{\hspace{1em}(13)}$$
  其中${\mathbf{x}}_k$是采集第$k$张图像时的IMU状态，由PVQ状态量和IMU偏置组成．${\lambda }_l$是第$l$个特征的逆深度(此深度是特征在第一次出现时定义的深度)．${\mathbf{x}}_c^b$是相机与IMU之间的外参．
• 视觉惯性BA公式
  $$\underset{\mathbf{\chi }}{\min }\left\{{\Vert {\mathbf{r}}_p-{\mathbf{H}}_p\mathbf{\chi }\Vert }^2+{\sum _{k\in B}\Vert {\mathbf{r}}_B({\hat{\mathbf{z}}}_{b_{k+1}}^{b_k},\mathbf{\chi })\Vert }_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2+\sum _{(l,j)\in C}\rho ({\Vert {\mathbf{r}}_C({\hat{\mathbf{z}}}_l^{c_j},\mathbf{\chi })\Vert }_{{\mathbf{P}}_l^{c_j}}^2)\right\}\text{\hspace{1em}(14)}$$
  式中$\rho (.)$为Huber norm损失函数，为了抑制噪声的影响，定义如下:
  $$\rho (s)=\{\begin{array}{c}s\\ 2\sqrt{s}-1\end{array}\begin{array}{c}s\le 1\\ s>1\end{array}\text{\hspace{1em}(15)}$$
  式中${\mathbf{r}}_B,{\mathbf{r}}_C$分别是对于IMU和视觉测量的残差项，后面会详细介绍．$[{\mathbf{r}}_p,{\mathbf{H}}_p]$是来自于边缘化过程中的先验信息，后面也有介绍．该视觉惯性BA公式的求解是个最小二乘优化问题，使用Ceres-solver优化库进行求解．

B. IMU测量残差

以滑窗内两个连续帧$b_k,b_{k+1}$进行说明，可对IMU测量残差进行如下定义:
$${\mathbf{r}}_B=\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{b_{k+1}}^{b_k}\\ \delta {\mathbf{\beta }}_{b_{k+1}}^{b_k}\\ \delta {\mathbf{\theta }}_{b_{k+1}}^{b_k}\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_{}\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_w^{b_k}({\mathbf{p}}_{b_{k+1}}^w-{\mathbf{p}}_{b_k}^w+\frac{1}{2}{\mathbf{g}}^w\Delta t^2-{\mathbf{v}}_{b_k}^w\Delta t)-{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}\\ {\mathbf{R}}_w^{b_k}({\mathbf{v}}_{b_{k+1}}^w+{\mathbf{g}}^w\Delta t-{\mathbf{v}}_{b_k}^w)-{\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k}\\ 2[{{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w\otimes ({\mathbf{\gamma }}_{b_{k+1}}^{b_k}{)}^{-1}{]}_{xyz}\\ {\mathbf{b}}_{a_{b_{k+1}}}-{\mathbf{b}}_{a_{b_k}}\\ {\mathbf{b}}_{w_{b_{k+1}}}-{\mathbf{b}}_{w_{b_k}}\end{array}\right]\text{\hspace{1em}(16)}$$
式中$[.{]}_{xyz}$指代的是将四元素转换成轴角表示，$[{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k},{\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k},{\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}]$是两个相邻图像帧之间的IMU测量值进行预积分的结果．

C.视觉测量残差

• 残差理解
  与传统针孔相机定义重投影误差在一个常见的图像平面上不一样，我们定义相机测量残差在一个单位球上面．原因是因为几乎所有类型的摄像机（包括广角，鱼眼或全向摄像机）的光学元件都可以建模为连接单位球表面的单位射线。
• 定义
  假设第$l$个特征首先是在第$i$张图像中被看到，则在第$j$张图像中观察到特征$l$的测量残差可定义如下:
  $$\begin{array}{rl}& {\mathbf{r}}_C({\hat{\mathbf{z}}}_l^{c_j},\mathbf{\chi })=[{\mathbf{b}}_1,{\mathbf{b}}_2{]}^T\cdot ({\widehat{\bar{\mathbf{\varrho }}}}_l^{c_j}-\frac{{\mathbf{\varrho }}_l^{c_j}}{\Vert {\mathbf{\varrho }}_l^{c_j}\Vert })\\ & {\widehat{\bar{\mathbf{\varrho }}}}_l^{c_j}={\pi }_c^{-1}(\left[\begin{array}{c}{\hat{u}}_l^{c_j}\\ {\hat{v}}_l^{c_j}\end{array}\right])\\ & {\mathbf{\varrho }}_l^{c_j}={\mathbf{R}}_b^c({\mathbf{R}}_w^{b_j}({\mathbf{R}}_{b_i}^w({\mathbf{R}}_c^b{\frac{1}{\lambda }}_l{\pi }_c^{-1}(\left[\begin{array}{c}{\hat{u}}_l^{c_i}\\ {\hat{v}}_l^{c_i}\end{array}\right])+{\mathbf{p}}_c^b)+{\mathbf{p}}_{b_i}^w-{\mathbf{p}}_{b_j}^w)-{\mathbf{p}}_c^b)\end{array}\text{\hspace{1em}(17)}$$
  式中$[{\hat{u}}_l^{c_i},{\hat{v}}_l^{c_i}]$是第$l$个特征在第$i$张图像中第一次被观察到的测量值，$[{\hat{u}}_l^{c_j},{\hat{v}}_l^{c_j}]$是第$l$个特征在第$j$张图像中被观察到的测量值，${\pi }_c^{-1}$是反投影函数(将像素值通过内参投影到一个单位向量)．由于视觉残差的自由度为2，于是我们按下图所示方式将残差向量投影到以$[{\mathbf{b}}_1,{\mathbf{b}}_2]$为基向量的切平面上，最后生成公式17所示的残差定义．
  

D.边缘化

论文中写的不够详细，不如直接看大神贺一家的博客https://blog.csdn.net/heyijia0327/article/details/52822104

重定位

我们使用滑窗策略和边缘化策略方法限制了算法的复杂性，但会引入累积漂移问题．为了消除漂移，提出了一个融合了单目VIO的紧耦合重定位模块．该模块首先进行回环检测，判断是否观测到已经观测过的场景，然后建立回环帧与当前帧之间的特征关系．随后，这些特征关系将会被融入单目VIO模块，最后能够利用最小资源实现无漂移状态估计．多特征的多观测被直接用于重定位，能够提高精度和更好的状态估计顺滑性．上述过程如下图所示．

A.回环检测

我们利用前沿的词袋位置识别方法DBoW2进行回环检测．除了被用于单目VIO的角点特征外，另外500个角点也被检测，同时利用BRIEF描述器对角点进行描述．额外的角点特征被使用可以在回环检测时实现更好的召回率．描述子将会被作为单词在视觉数据集里面进行检索．DBoW2返回回环检测到的并经过时间和空间一致性检查的候选帧．我们维护所有的用于特征检索的描述子，但将原始图像丢弃，从而节约内存资源．

B.特征检索

当一个回环检测到，可以通过检索局部滑窗内的帧和回环候选帧之间的特征一致性建立两者之间的联系，其中一致性的检索是通过特征描述子匹配进行的．描述子匹配可能会引入误匹配，于是我们使用了两步几何离群值丢弃策略，减少误匹配．

• 2-D-2-D: 基于基础矩阵的RANSAC方法去除误匹配
• 3-D-2-D: 基于PnP方法的RANSAC方法去除误匹配

C.紧耦合重定位

重定位过程有效的对齐了当前滑窗和先前的位姿．在重定位过程中，我们将回环帧的位姿置为静态的，将滑窗内的所有IMU测量、局部视觉测量和检索到的特征一起进行优化．我们同公式(17)一样可以轻松的写出视觉测量模型残差，其中唯一的差异是回环帧的位姿$({\hat{\mathbf{q}}}_v^w,{\hat{\mathbf{p}}}_v^w)$是直接来自于位姿图或过去的里程计输出，而且是静态的．最后，我们只需要在公式(14)的非线性代价函数中添加回环误差项构成新的误差函数，具体函数如下:
$$\underset{\mathbf{\chi }}{\min }\left\{{\Vert {\mathbf{r}}_p-{\mathbf{H}}_p\mathbf{\chi }\Vert }^2+{\sum _{k\in B}\Vert {\mathbf{r}}_B({\hat{\mathbf{z}}}_{b_{k+1}}^{b_k},\mathbf{\chi })\Vert }_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2+\sum _{(l,j)\in C}\rho ({\Vert {\mathbf{r}}_C({\hat{\mathbf{z}}}_l^{c_j},\mathbf{\chi })\Vert }_{{\mathbf{P}}_l^{c_j}}^2)+\sum _{(l,v)\in L}\rho ({\Vert {\mathbf{r}}_C({\hat{\mathbf{z}}}_l^v,\mathbf{\chi },{\hat{\mathbf{q}}}_v^w,{\hat{\mathbf{p}}}_v^w)\Vert }_{{\mathbf{P}}_l^{c_v}}^2)\right\}$$
式中$L$是在回环帧中检索到的特征的集合．$(l,v)$指的是第$l$个特征被第$v$个回环帧观测到．注意，虽然代价函数与公式(14)有轻微的不同，但状态的维度并没有发生变化，因为回环帧的位姿是被置为静态的．当与当前滑窗建立了多个回环，我们优化将会使用所有的相关帧的相关特征．这将会给重定位过程引入多视图约束，结果将会更精确更顺滑．

全局位姿图优化和地图重用

A. 四自由度累积漂移方向

得益于重力的惯性测量，在世界坐标系中roll和pitch是绝对状态，而$x,y,z,yaw$是相对与参考系的相对状态．因此累积漂移只发生在$x,y,z,yaw$四个状态．为了充分利用这个有效信息来有效的矫正漂移，我们固定$roll,pitch$值，只执行4自由度的位姿图优化

B. 添加关键帧到位姿图中

在VIO过程之后，关键帧将会被添加到位姿图中．每个关键帧作为位姿图中的一个节点，并且通过两种类型的边与其它节点相连．两种边类型如下:

• 顺序边
  一个关键帧与它的前几帧建立几个顺序边．一个顺序边表示两个关键帧之间的相对转换，是直接通过VIO获得的．考虑关键帧$i$和它的一个先前帧$j$，顺序边只包含相对位置${\hat{\mathbf{p}}}_{ij}^i$和$yaw$角${\hat{\psi }}_{ij}$.
  $$\begin{array}{rl}{\hat{p}}_{ij}^i& ={\hat{\mathbf{R}}}_i({\hat{\mathbf{p}}}_j^w-{\hat{\mathbf{p}}}_i^w)\\ {\hat{\psi }}_{ij}& ={\hat{\psi }}_j-{\hat{\psi }}_j\end{array}\text{\hspace{1em}(19)}$$
• 回环边
  如果关键帧有一个回环连接，它通过一个回环边连接回环帧．类似的，回环边只包含4自由度相对位姿变换，同公式(19)一致．回环边的值是通过重定位获得的．

c. 4自由度位姿图优化

我们定义第$i,j$帧之间的边的残差如下:
$$\mathbf{r}({\mathbf{p}}_i^w,{\psi }_i,{\mathbf{p}}_j^w,{\psi }_j)=\left[\begin{array}{c}\mathbf{R}({\hat{\varphi }}_i,{\hat{\theta }}_i,{\psi }_j{)}^{-1}({\mathbf{p}}_j^w-{\mathbf{p}}_j^w-{\hat{\mathbf{p}}}_{ij}^i)\\ {\psi }_j-{\psi }_i-{\hat{\psi }}_{ij}\end{array}\right]\text{\hspace{1em}(20)}$$
式中${\hat{\varphi }}_i,{\hat{\theta }}_i$是固定的$roll,pitch$角度估计值，是直接通过VIO获得的．
整个包含顺序边和回环边的位姿图是通过最小化下式代价函数获得的:
$$\underset{\mathbf{p},\psi }{\min }\left\{\sum _{(i,j)\in S}{\Vert {\mathbf{r}}_{i,j}\Vert }^2+\sum _{(i,j)\in L}\rho ({\Vert {\mathbf{r}}_{i,j}\Vert }^2)\right\}\text{\hspace{1em}(21)}$$
式中$S$是所有顺序边的集合，$L$是所有回环边的集合．虽然紧耦合重定位已经帮我们消除了错误的回环，我们任然添加Huber norm $\rho (\cdot )$进一步减少任意错误回环的影响．相反的，我们对顺序边使用任何损失函数，因为这些边是直接通过VIO获取的，已经包含该了足够多的离群值丢弃策略．
位姿图优化和重定位同步运行在两个不同的线程中，这保证了对最新优化的位姿图的快速使用．同样额，虽然当前位姿图优化没有完成，重定位任然可以使用已经存在的位姿图进行．

D. 位姿图融合

位姿图不仅能够优化当前的地图，而且也能够融合当前的地图和先前的地图．如果我们已经加载了一个先前构建的地图，并且在两幅地图中检测到了回环，我们就可以融合它们到一起．因为所有的边是相对约束，位姿图通过回环联系自动融合两幅地图．

E. 位姿图保存

我们的位姿图结构是非常简单的．我们只需要保存节点、边和每个关键帧的描述子．原始图像是丢弃的，为了减少内存的消耗．具体一点，我们保存的第$i$关键帧是:
$$[i,{\hat{\mathbf{p}}}_i^w,{\hat{\mathbf{q}}}_i^w,v,{\hat{\mathbf{p}}}_{iv}^i,{\hat{\psi }}_{iv},\mathbf{D}(u,v,des)]\text{\hspace{1em}(22)}$$
式中$i$是帧索引，${\hat{\mathbf{p}}}_i^w,{\hat{\mathbf{q}}}_i^w$是关键帧的位置和姿态，通过VIO获得．如果这一帧有回环帧，$v$是回环帧的索引．${\hat{\mathbf{p}}}_{iv}^i,{\hat{\psi }}_{iv}$是这两帧之间的相对位置和$yaw$角，通过重定位获得．$\mathbf{D}(u,v,des)$是特征集合，每个特征包含2自由度位置和它的描述子．

F. 位姿图加载

我们使用同保存格式相同的格式加载关键帧．每个关键帧是位姿图中的一个节点．节点的初始位姿是${\hat{\mathbf{p}}}_i^w,{\hat{\mathbf{q}}}_i^w$．回环边直接通过回环信息${\hat{\mathbf{p}}}_{iv}^i,{\hat{\psi }}_{iv}$建立．每个关键帧与它的相邻关键帧建立顺序边．在加载完位姿图之后，我们执行全局4自由度的位姿图优化．位姿图保存和加载的时间与位姿图的尺寸成正比．