手写VIO课后作业（一）

1、VIO 文献阅读

阅读 VIO 相关综述文献，回答以下问题：
• 视觉与 IMU 进行融合之后有何优势？
• 有哪些常见的视觉 +IMU 融合方案？有没有工业界应用的例子？
• 在学术界，VIO 研究有哪些新进展？有没有将学习方法用到 VIO中的例子？
你也可以对自己感兴趣的方向进行文献调研，阐述你的观点。
Jianjun Gui et al. “A review of visual inertial odometry from filtering and optimisation perspectives”

（1）视觉与 IMU 进行融合之后有何优势

首先分别介绍一下各自的优缺点。IMU可以高频率提供自身的运动信息，从而对快速运动很鲁棒。其次，IMU的信息也不受环境，纹理等变化的影响，还能够测得绝对尺度。但是IMU存在零偏，长时间积分会导致位姿发散，所以IMU适合短时间快速的运动。视觉可以提供丰富的地图信息，且它的测量信息不受零偏的影响。在视觉slam中直接测量旋转和平移，不需要做积分处理，因此，当相机静止的时候，轨迹不会像IMU一样发散，但是相机在快速运动时会产生图像的模糊，从而使得图像无法匹配，然后导致定位失败。视觉信息会受到图像遮挡、环境伦理、环境光照、动态物体干扰等因素的影响。而且在单目视觉的情况下，会有尺度不确定和纯旋转这些问题导致无法估计位姿。
由此可见，视觉和IMU存在一定的互补，互补之后就有了一些优点。视觉slam在快速运动或者图像遮挡、纹理光照等干扰的情况下运动追踪失败可以靠IMU进行定位，从而提高系统的鲁棒性。在运动的过程中如果出现了图像发生巨大变化，本质上我们没有办法知道是相机自身发生的变化，还是外界环境发生了变化，但是我们可以使用IMU信息来感知自身的运动。从某种意义上来说，减轻了动态物体的一个影响。通过融合视觉信息和IMU信息，可以对旋转运动进行估计并恢复真实的尺度。而且借助IMU高频率的一个采样，可以提高系统的输出频率。

总结：

• IMU适合计算短时间、快速的运动。
• 视觉适合计算长时间、慢速的运动。
• 可利用视觉定位信息来估计IMU的零偏，减少IMU由零偏导致的发散和累积误差。
• IMU可以为视觉提供快速运动时的定位。

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

融合方案

• MSCKF：紧耦合，FAST+光流前端；后端用重投影误差，无回环检测。
• OKVIS：紧耦合，前端使用多尺度Harris提取特征点，使用BRISK作为描述子，后端使用ceres进行非线性优化完成状态估计。
• ROVIO：紧耦合，基于稀疏图像块的EKF滤波实现VIO，基于单目的，前端是用FAST+光流，后端是EKF滤波。
• VINS：紧耦合，前端Harris+光流，有回环检测，通过非线性优化一个滑窗内的KF等。
  

工业界应用的例子

（1）VIO 在工业中广泛应用于无人机以及移动机器人的导航，自动驾驶技术中目前也开始增加VIO 的技术使用，目前自动驾驶广泛应用的是 GPS 与 IMU 的组合导航，但是在某些场合场合，GPS 信号弱，不能提供良好的导航效果，于是增加相机与 IMU 的融合。
（2）AR(Augmented Reality) 增强现实技术是通过带有视觉传感器的设备实现在物理真实场景中添加虚拟信息，实现现实和虚拟场景的交互。例如 Apple 的 ARKit 在移动设备上实现的 AR，ARKit是通过 VIO 实现移动设备在空间中的精确定位。
（3）VR(Virtual Reality) 虚拟现实技术则是通过虚拟现实头盔投射虚拟信息，给人身临其境的感觉，将使用者在实际空间中的移动反映到虚拟空间上，可以很大程度地提高交互体验，其中的Inside-out 定位目前广泛采用的方案就是视觉惯性传感器融合实现 SLAM 的 VINS 算法。

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

新进展

深度学习用到VIO

基于深度学习的，大体上分为三大类

• 用DL替换传统方法中的一个或几个模块
• 在传统SLAM之上加入语义信息，从而构建语义级地图
• 端到端的SLAM

2、四元数和李代数更新

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

请编程验证对于小量 ω = [0.01, 0.02, 0.03]T，两种方法得到的结果非常接近，实践当中可视为等同。因此，在后文提到旋转时，我们并不刻意区分旋转本身是 q 还是 R，也不区分其更新方式为上式的哪一种。

代码：

#include <iostream>
#include <cmath>
#include <time.h>

#include<Eigen/Core>
#include<Eigen/Geometry>

#include"sophus/so3.h"
#include"sophus/se3.h"

int main(int argc, char ** argv){

    //1.set a random R,q
    srand((unsigned)time(NULL));
    int R_theat = rand()%360;
    Eigen::Vector3d R_axis = Eigen::Vector3d::Random().normalized();
    Eigen::AngleAxisd rotation_vector(M_PI*R_theat/180.0, R_axis);
    Eigen::Matrix3d R = rotation_vector.toRotationMatrix();
    Eigen::Quaterniond q(R);

    //2.set w
    Eigen::Vector3d w(0.01,0.02,0.03);

    //3.update R
    Sophus::SO3 SO3_R(R);
    Sophus::SO3 SO3_updated = SO3_R * Sophus::SO3::exp(w);
    std::cout<<"after w, the Rotation matrix updated = "<<std::endl<<SO3_updated.matrix()<<std::endl;

    //4.update q
    Eigen::Quaterniond dq(1, w[0]/2, w[1]/2, w[2]/2);
    Eigen::Quaterniond q_updated = q * (dq.normalized()); //四元数使用之前要归一化
    std::cout<<"after w, the q = "<<std::endl<<q_updated.matrix()<<std::endl;

    //5.compute the diff between R & q.
    Eigen::Matrix3d R_diff = SO3_updated.matrix() - q_updated.toRotationMatrix();
    std::cout<<"difference is = "<<std::endl<<R_diff<<std::endl;
}

注意：单位四元数表示旋转，如果不是单位四元数有一个缩放作用，所以归一化要放在更新之前。

结果：

从结果上面来看，两者的运算差值都是在十的负六次方这个级别，因此可以认为两者是等价的。

3、其他导数

使用右乘 so(3)，推导以下导数：

（1）

或

第一题可以针对SO3加上又扰动。也可以针对李代数，加上又扰动两者之间的推导，结果只差了一个右乘雅克比。但题中说的是so3，即针对李代数。

（2）

或

记住一些性质，方便公式的推导：