深蓝学院-手写VIO作业-第一章

一、VIO文献阅读

阅读VIO 相关综述文献[a]，回答以下问题：

1. 视觉与IMU进行融合之后有何优势？

   • 视觉传感器在大多数纹理比较丰富的场景中效果较好，但是如果遇到白墙，或者纯色的一些特殊场景，基本上无法提取特征，也就无法工作；并且频率不能太高，15-60Hz居多。
   • IMU存在漂移现象，随着时间的推移，误差累积越来越多，但在短时间内，IMU的精度很高，典型的6轴IMU频率大于或等于100Hz。

   我的理解是，因为视觉和IMU都有各自相应的优势和缺点，整合两个传感器的优势，取长补短。因此，融合之后的优势是：

   • 视觉与IMU的融合可以借助IMU较高的采样频率，进而提高系统的输出频率。

   • 视觉与IMU的融合可以提高视觉的鲁棒性，如视觉SLAM因为某些运动或场景出现的错误结果。

   • 视觉与IMU的融合可以有效的消除IMU的积分漂移。

   • 视觉与IMU的融合能够校正IMU的Bias。

   • 单目与IMU的融合可以有效解决单目尺度不可观测的问题。

   • 可以应对快速的运动变化，相机在快速运动过程中会出现运动模糊。

2. 有哪些常见的视觉+IMU 融合方案？有没有工业界应用的例子？

   常见的融合方案：
   

   2020年推出的ORB-SLAM3方案，提供了双目+IMU的融合方案，并且ORB-SLAM3在深蓝学院公开课也有介绍。

   工业界应用

   VIO主要是应用自动驾驶和AR、VR领域；比如可以使用手机进行AR换鞋（京东淘宝上有些店铺是支持AR试鞋的）。以硬件为主打的XR头盔设备，目前在工业领域都一些落地的场景，如西门子的AR头盔的工业展示，飞机驾驶员的培训。

3. 在学术界，VIO 研究有哪些新进展？有没有将学习方法用到VIO中的例子？

​ 目前VIO和其他传感器的融合也是一个比较受欢迎的研究热点，比如VIO和激光雷达的融合，和GPS的融合。

• 2019, RAL, Visual-Inertial Localization with Prior LiDAR Map Constraints --视觉 VIO 和激光地图。
• 2018, arxiv, A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors --VINS-Mono 的扩展版，能 融合 GPS、单目、双目等信息。
• 2017, ICRA, Vins on wheels --系统分析了 VIO + 轮速计系统的可观性。
• 2019, ICRA, Visual-Odometric Localization and Mapping for Ground Vehicles Using SE(2)-XYZ Constraints --SE2-XYZ 的模型来对地 面轮速机器人进行参数化。

VIO+深度学习

• VINet : Visual-inertial odometry as a sequence-to-sequence learning problem.就整体而言，VINet是首次使用DL的框架来解决VIO问题，目前所披露的实验表现出了一定的实用价值。
• Unsupervised Deep Visual-Inertial Odometry with Online Error Correction for RGB-D Imagery.此方法可以在没有Camera-IMU外参的情况下基于学习进行VIO网络学习整合IMU测量并生成估计轨迹，然后根据相对于像素坐标的空间网格的缩放图像投影误差的雅可比行列式在线校正。

参考博客：视觉惯性里程计 综述

题目：A review of visual inertial odometry from filtering and optimisation perspectives

期刊：Advanced Robotics | Volume 29,2015 - Issue 20

作者及研究机构信息：

翻译：VIO综述论文

二、四元数和李代数更新

课件提到了可以使用四元数或旋转矩阵存储旋转变量。当我们用计算出来的$\omega$对某旋转更新时，有两种不同方式:

$$\begin{gathered} R\leftarrow R\exp ({\omega }^{\wedge }) \\ 或q\leftarrow q\otimes [1,\frac{1}{2}\omega {]}^T \end{gathered}$$
请编程验证对于小量$\omega$ = [0.01, 0.02, 0.03]T，两种方法得到的结果非常接近，实践当中可视为等同。因此，在后文提到旋转时，我们并不刻意区分旋转本身是q 还是R，也不区分其更新方式为上式的哪一种。

代码编写

按照公式进行编程，参照了课本p48和p87页代码中对于eigen库和sophus库的使用。

旋转向量参照课本上的例子，将其设置为沿z轴旋转45度。利用旋转矩阵构造对应的四元数，利用 Sophus::SO3d::hat可以求得旋转向量的反对称矩阵。

具体代码如下：

int main()
{
    //旋转矩阵
    Matrix3d rotation_matrix = Matrix3d::Identity();
    AngleAxisd rotation_vector(M_PI/4, Vector3d(0,0,1));  //表示沿Z轴旋转45度
    rotation_matrix = rotation_vector.toRotationMatrix();

    cout<<"R: "<<endl<<rotation_matrix<<endl<<endl;

    //构造对应的四元数
    Eigen::Quaterniond q(rotation_matrix);
    cout << "q :"<< endl << q.coeffs().transpose() <<endl<<endl;

    //使用对数映射获得它的李代数
    Sophus::SO3d SO3_R(rotation_matrix);
    cout << "so3 :"<< endl << SO3_R.log().transpose() <<endl<<endl;

    //更新量omega
    Eigen::Vector3d omega(0.01, 0.02, 0.03); //更新量

    // 李代数更新
    Matrix3d skew_symmetric_matrix = Sophus::SO3d::hat(omega).matrix();
    cout<<"SO3 hat= "<<endl<< skew_symmetric_matrix<<endl<<endl;
    Sophus::SO3d SO3_updated = SO3_R * Sophus::SO3d::exp(omega);
    cout<<"SO3 updated = "<<endl<<(SO3_R * Sophus::SO3d::exp(omega)).matrix()<<endl<<endl;

    // 四元数更新
    Eigen::Quaterniond q_update(1, omega(0)/2, omega(1)/2, omega(2)/2);
    Eigen::Quaterniond q_updated = q * q_update.normalized();
    cout<<"Quaterniond updated = "<< endl << q_updated.toRotationMatrix() <<endl<<endl;

    //衡量两种方法之间的差距
    cout<<"disparity = "<< endl <<SO3_updated.matrix().transpose()*q_updated.toRotationMatrix()<<endl;
}

最终运行结果截图：

从实验结果可以看出，两种不同方式得到的$\omega$对某旋转更新结果差别很小，（这里需要注意的一点是，因为旋转矩阵是在李群上的，李群是没有加减法的，所以可以使用第一个R的逆或者转置矩阵乘第二个R矩阵，越接近单位矩阵，则说明两个矩阵之间的差距越小）差距在$10^{-6}$以内，因此并不刻意区分旋转本身是q 还是R。

三、其他导数

使用右乘$\mathfrak{s}\mathfrak{o}(3)$，推导以下导数：

1.

$$\frac{d({\mathbf{R}}^{-1}p)}{d\mathbf{R}}$$

右扰动：
$$\begin{array}{rl}\frac{d(R^{-1}p)}{dR}& =\frac{d(R^{-1}p)}{d\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(R\exp ({\phi }^{\wedge }){)}^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(\exp ({\phi }^{\wedge }){)}^{-1}{R}^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp (-{\phi }^{\wedge })R^{-1}p-R^{-1}p}{\phi }\\ 泰勒展开& \approx \underset{\phi \to 0}{\lim }\frac{(I-{\phi }^{\wedge })R^{-1}p-R^{-1}p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-{\phi }^{\wedge }{R}^{-1}p}{\phi }\\ 根据slam课程中表1中提供的性质u^{\wedge }v=-v^{\wedge }u可得：& =\underset{\phi \to 0}{\lim }\frac{(R^{-1}p{)}^{\wedge }\phi }{\phi }\\ & =(R^{-1}p{)}^{\wedge }\end{array}$$

2.

$$\frac{dln({\mathbf{R}}_1{\mathbf{R}}_2^{-1}{)}^{\vee }}{d{\mathbf{R}}_2}$$

这里需要用BCH线性近似表达：
$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{\mathcal{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& 当{\varphi }_1为小量时\\ {\mathcal{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& 当{\varphi }_2为小量时\end{array}$$
还用到了如下性质：
$$\begin{gathered} (\mathbf{C}u{)}^{\wedge }\equiv \mathbf{C}{u}^{\wedge }{\mathbf{C}}^T \\ \exp ((\mathbf{C}u{)}^{\wedge })\equiv \mathbf{C}\exp (u^{\wedge }){\mathbf{C}}^T \\ 旋转矩阵R^{-1}=R^T \end{gathered}$$

通过右扰动的方式计算上边的公式：
$$\begin{array}{rl}\frac{d\ln (R_1{R}_2^{-1}{)}^{\vee }}{d{R}_2}& =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1(R_2\exp ({\phi }_2^{\wedge }){)}^{-1}{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1\exp (-{\phi }_2^{\wedge })R_2^{-1}{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1{R}_2^{-1}{R}_2\exp (-{\phi }_2^{\wedge })R_2^T{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =\underset{{\phi }_2\to 0}{\lim }\frac{\ln (R_1{R}_2^{-1}\exp ((-R_2{\phi }_2{)}^{\wedge }){)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ BCH近似& \approx \frac{{\mathcal{J}}_r(\ln (R_1{R}_2^{-1}{)}^{\vee }{)}^{-1}(-R_2{\phi }_2)+\ln (R_1{R}_2^{-1}{)}^{\vee }-\ln (R_1{R}_2^{-1}{)}^{\vee }}{{\phi }_2}\\ & =-{\mathcal{J}}_r(\ln (R_1{R}_2^{-1}{)}^{\vee }{)}^{-1}\cdot R_2\end{array}$$

代码地址：https://gitee.com/ximing689/vio-learning.git