视觉−惯性里程计(VIO)融合SLAM方案

第一章

1. 视觉与IMU融合后的优势？

  传感器融合是一个趋势，也或者说是一个妥协的结果。主要的原因还是由于单一的传感器不能适用所有的场景，所以寄希望于通过多个传感器的融合达到理想的定位效果。目前主流的传感器融合方案包括：视觉+IMU、视觉+激光雷达、IMU+差分GPS+激光雷达。

视觉+IMU融合方案

  视觉传感器：在大多数纹理丰富的场景中效果很好，但是如果遇到玻璃，白墙等特征较少的场景，基本上无法工作；快速运动时定位跟踪容易丢失；单目视觉无法测量尺度。但是由于视觉不产生漂移，可以直接测量旋转平移。
  IMU：由于零偏和噪声的存在，导致其长时间使用有非常大的累积误差；低精度IMU长时间积分位姿容易发散，而高精度的价格普遍较贵；同时IMU 有输出频率高、能输出6DoF测量信息等优点，在短时间内，其相对位移数据有很高的精度。

  因此视觉和 IMU 定位方案存在一定互补性质；所以当视觉传感器在短时间内快速运动失效时，融合IMU数据，能够为视觉提供短时的精准定位，同时利用视觉定位信息来估计IMU的零偏，减少IMU由零偏导致的发散和累积误差。通过二者的融合，可以解决视觉位姿估计输出频率低的问题，同时位姿估计精度有一定的提高，整个系统也更加鲁棒。

2. 常见的视觉和IMU融合方案？工业界的应用例子

  视觉和IMU融合可以分为基于滤波和基于优化两种方法。按照是否把图像特征信息加入状态向量来进行分类，可以分为松耦合和紧耦合两大类。

  松耦合：将视觉传感器和 IMU分别计算得到的位姿直接进行融合，融合过程对二者本身不产生影响，作为后处理方式输出，一般通过 EKF 进行融合。
  紧耦合：将视觉传感器和 IMU 的状态通过一个优化滤波器合并在一起，紧耦合需要把图像特征加入到特征向量中，共同构建运动方程和观测方程，然后进行状态估计，最终得到位姿信息的过程，其融合过程本身会影响视觉和 IMU 中的参数（如 IMU 的零偏和视觉的尺度）。

  基于滤波的融合方案：

• 松耦合：苏黎世联邦理工学院(eth)的 SSF(融合IMU和单个视觉传感器)和 MSF(融合IMU和多个位置传感器)；
• 紧耦合：比较经典的算法是MSCKF，ROVIO；

  基于优化的融合方案：

• 松耦合： 基于松耦合优化的工作不多。 Gabe Sibley在IROS2016的一篇文章《Inertial Aided Dense & Semi-Dense Methods for Robust Direct Visual Odometry》提到了这个方法。简单来说就是把VO计算的位姿变换添加到IMU的优化框架里面。
• 紧耦合：OKVIS（多目+IMU）、 VINS-Mono、VINS-Fusion、VI-ORB；

  工业界应用举例：
  VIO目前实现比较好的是VINS、OKVIS和MSCKF_VIO。前两个是基于非线性优化的方案而且框架比较相似，后者是基于滤波优化的方案，也是Google Tango上使用的方法。同时，VIO在工业上的应用还有苹果的ARKit和谷歌的ARCore。

3. 学术界VIO研究进展如何？将学习的方法用到VIO的例子

  传统VIO：

1. PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features（Sensors-2018）
     为了解决相机轨迹估计问题，建立基于惯性测量和视觉观测的三维结构图，提出了点-线视觉惯性里程计（PL-VIO），一种利用点和线特征的紧密耦合的单目视觉惯性里程计系统。为了实现惯性测量单元（IMU）和视觉传感器信息的紧密有效融合，在滑动窗口优化框架下，通过最小化代价函数，将预先集成的IMU误差项与点线重投影误差项相结合，对状态进行优化。
2. Visual-Inertial Odometry with Point and Line Features（IROS-2019）
     提出了一种使用点和线的紧密耦合的单目视觉惯性导航系统（VINS），并采用退化运动分析进行三维线三角测量。基于图像的线段测量，提出了 两种基于滑动窗口的三维线三角测量算法。

  深度学习+VIO

1. VINet: Visual-Inertial Odometry as a Sequence-to-Sequence Learning Problem（AAAI-2017）
     VINet是首次使用端到端可训练的深度神经网络架构尝试解决机器人领域视觉惯性里程计的问题，目前所披露的实验表现出了一定的实用价值。与传统方法相比具有许多优势。它消除了对摄像机和IMU进行繁琐的手动同步的需要，也消除了对IMU和摄像机之间进行手动校准的需要。另一个优势是，模型可以自然合并特定领域的信息，大大减轻了漂移。
2. DeepVIO: Self-supervised Deep Learning of Monocular Visual Inertial Odometry using 3D Geometric Constraints（IROS-2019）
     第一次使用自监督深度学习网络（DeepVIO），端到端的单目VIO（从双目中获取监督信息）。DeepVIO通过直接融合2D光流特征（OFF）和惯性测量单元（IMU）数据提供绝对轨迹估计。
3. Unsupervised Depth Completion from Visual Inertial Odometry（ICRA-2019）
     采用了无监督的KITTI深度作为基准深度数据集，首次提出了视觉惯性+深度数据集的融合方式，进一步增强视觉和惯性传感器的互补优势。
4. Selective Sensor Fusion for Neural Visual-Inertial Odometry（CVPR-2019）
     针对目前视觉惯性里程计（VIO）的深度学习方法很少结合稳健的融合策略去处理不完美的输入数据。提出了一种新的单目端到端VIO传感器融合框架，该框架融合了单目图像和惯性测量数据，以估计轨迹，同时提高了对真实问题的鲁棒性，如数据丢失和损坏或传感器同步不良等问题。还提出了两种基于不同掩蔽策略的融合模式：确定软融合和随机硬融合，且实验结果证明在数据被破坏的情况下，该融合策略与直接融合相比，具有更好的性能。

4. 使用右乘$so(3)$，推导下面导数

4.1 解：
  因为李代数$so(3)$与$SO(3)$的关系由指数映射给定：
$$R=exp({\varphi }^{\bigwedge })$$
  则旋转微小量： $\Delta R=exp({\phi }^{\bigwedge })$
  在优化带有旋转的函数时，通常计算一个微小增量 $\varphi \in so(3)$,然后使用右乘李代数进行更新：$R^{{}^{\prime }}=Rexp({\varphi }^{\bigwedge })$

$$\begin{array}{rl}\frac{d(R^{-1}P)}{dP}& =\underset{\phi \to 0}{\lim }\frac{(Rexp({\phi }^{\bigwedge }){)}^{-1}P-R^{-1}P}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\bigwedge }{)}^{-1}{R}^{-1}P-R^{-1}P}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\bigwedge }{)}^T{R}^{-1}P-R^{-1}P}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\bigwedge }{)}^T{R}^{-1}P-R^{-1}P}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{({\phi }^{\bigwedge }{)}^T{R}^{-1}P}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-{\phi }^{\bigwedge }{R}^{-1}P}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(R^{-1}P{)}^{\bigwedge }\phi }{\phi }\\ & =(R^{-1}P{)}^{\bigwedge }\end{array}$$

上式中用到的知识：

• 由于${\phi }^{\bigwedge }$是反对称函数，因此$({\phi }^{\bigwedge }{)}^T=-{\phi }^{\bigwedge }$；
• 由于旋转矩阵$R$是正交矩阵，即$R^{-1}=R^T$,对应的李代数$[exp({\phi }^{\bigwedge }){]}^{-1}=[exp({\phi }^{\bigwedge }){]}^T$；
• 反对称矩阵与向量的相乘相当于两个向量的叉乘（前后交换相乘顺序需变号），例如：$a\times b=a^{\bigwedge }b=-b\times a=-b^{\bigwedge }a$。必须保证一个反对称矩阵与一个向量，这里$R^{-1}P$表示向量（为一个整体）；

4.2 解：
$$\begin{array}{rl}\frac{dln(R_1{R}_2^{-1})}{d{R}_2}& =\underset{\phi \to 0}{\lim }\frac{ln(R_1[R_{2}exp({\phi }^{\bigwedge }){]}^{-1})-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1[exp((R_2\phi {)}^{\bigwedge })R_2{]}^{-1})-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}[exp((R_2\phi {)}^{\bigwedge }){]}^{-1})-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}[exp((R_2\phi {)}^{\bigwedge }){]}^T)-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}exp(((R_2\phi {)}^{\bigwedge }{)}^T))-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}exp(-(R_2\phi {)}^{\bigwedge }))-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}exp((-R_2\phi {)}^{\bigwedge }))-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1})+J_r^{-1}(ln(R_1{R}_2^{-1}))(-R_2\phi )-ln(R_1{R}_2^{-1})}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{J_r^{-1}(ln(R_1{R}_2^{-1}))(-R_2\phi )}{\phi }\\ & =-J_r^{-1}(ln(R_1{R}_2^{-1}))R_2\end{array}$$

上面用到的知识：

• $SO(3)$的伴随性质：$Rexp({\varphi }^{\bigwedge })R^T=exp((R\varphi {)}^{\bigwedge })$，第一行到第二行用到该性质；
• 由于 $(R_2\phi {)}^{\bigwedge }$ 是反对称矩阵(方阵)，因此$((R_2\phi {)}^{\bigwedge }{)}^T=-(R_2\phi {)}^{\bigwedge }=(-R_2\phi {)}^{\bigwedge }$ ；
• 那么第四行到第七行可以推导$[exp((R_2\phi {)}^{\bigwedge }){]}^T=exp(((R_2\phi {)}^{\bigwedge }{)}^T)=exp((-R_2\phi {)}^{\bigwedge })$；
• 第七到第十行可以利用BCH近似得到：$ln(Rexp({\varphi }^{\bigwedge }))=ln(R)+J_r^{-1}(ln(R))\varphi$；