从零手写VIO（一）

从零手写VIO第一讲作业
这是我自己总结的第一讲的思维导图：从零手写VIO第一讲思维导图

前面几个综述问题大部分 完全参考的大佬的博客https://blog.csdn.net/learning_tortosie/article/details/102408738,（懒癌又犯了），本博客设为转载，以示敬意。
公式推导部分和大佬博客略有不同，如有错误，希望指正。

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

要说视觉与IMU融合之后有何优势，首先要明白视觉与IMU各自的局限：

（1）IMU虽然可以测得加速度和角速度，但这些量都存在明显的漂移，使得积分两次得到的位姿数据非常不可靠。好比说，我们将IMU放在桌上不动，用它的读数积分得到的位姿也会漂出十万八千里。但是，对于短时间内的快速运动，IMU能够提供一些较好的估计。这正是相机的弱点。当运动过快时，（卷帘快门的）相机会出现运动模糊，或者两帧之间重叠区域太少以至于无法进行特征匹配，所以纯视觉SLAM非常害怕快速的运动。而有IMU，即使在相机数据无效的那段时间内，我们还能保持一个较好的位姿估计，这是纯视觉SLAM无法做到的。

（2）相比于IMU，相机数据基本不会有漂移。如果相机放在原地固定不动，那么（在静态场景下）视觉SLAM 的位姿估计也是固定不动的。所以，相机数据可以有效地估计并修正IMU读数中的漂移，使得在慢速运动后的位姿估计依然有效。

（3）当图像发生变化时，本质上我们没法知道是相机自身发生了运动，还是外界条件发生了变化，所以纯视觉SLAM 难以处理动态的障碍物。而IMU能够感受到自己的运动信息，从某种程度上减轻动态物体的影响。

（4）对于单目视觉SLAM，存在尺度不确定性，融合IMU后可以恢复尺度。

（5）纯视觉SLAM在容易受弱纹理场景和光照变化的影响，在定位失败时，可以依靠IMU进行短暂的定位。

综上，视觉与IMU融合之后会弥补各自的劣势，可利用视觉定位信息来估计IMU的零偏，减少IMU由零偏导致的发散和累积误差；IMU可以为视觉提供快速运动时的定位，以及因为某种因素（场景特征点较少，光照变化较大等）定位失败时。

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

（1）常见的视觉+IMU融合方案

MSCKF
OKVIS
ROVIO
VIORB
VINS-Mono，VINS-Mobile，VINS-Fusion
以上方案基本都在Github上开源了。

（2）工业界应用

Google：Tango，ARCore
Apple：ARKit
Microsoft：HoloLens
百度：DuMix AR

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

要找学术研究的新进展，可以去Google Scholar通过关键词（VIO，Visual-Inertial Odometry）和限定时间（如2019年以来）来检索，还可以在机器人，计算机视觉等顶会（如IROS，ICRA，CVPR）上检索。检索是第一步，接下来就要阅读题目和摘要，筛选有意义的研究，然后再选择性精读论文。

目前，我对学术论文关注较少，今天正好看到有人在知乎上发布了VIO新进展，就简单地搬运到这里。

（1）传统方法新进展

[1] Usenko V , Demmel N , Schubert D , et al. Visual-Inertial Mapping with Non-Linear Factor Recovery[J]. 2019.
[2] Shao W , Vijayarangan S , Li C , et al. Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping[J]. 2019.

（2）基于学习方法的例子

[1] Clark R, Wang S, Wen H, et al. VINet: Visual-Inertial Odometry as a Sequence-to-Sequence Learning Problem[C]//AAAI. 2017: 3995-4001.
[2] Chen, Changhao, et al. “Selective Sensor Fusion for Neural Visual-Inertial Odometry.” arXiv preprint arXiv:1903.01534 (2019).
[3] Shamwell, E. Jared, et al. “Unsupervised Deep Visual-Inertial Odometry with Online Error Correction for RGB-D Imagery.” IEEE transactions on pattern analysis and machine intelligence (2019).
[4] Lee, Hongyun, Matthew McCrink, and James W. Gregory. “Visual-Inertial Odometry for Unmanned Aerial Vehicle using Deep Learning.” AIAA Scitech 2019 Forum. 2019.
[5] Wang, Chengze, Yuan Yuan, and Qi Wang. “Learning by Inertia: Self-supervised Monocular Visual Odometry for Road Vehicles.” ICASSP 2019-2019 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP). IEEE, 2019.

4. 四元数和李代数更新

1. 代码：

#include <iostream>

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

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

using namespace std; 

int main( int argc, char** argv )
{
    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    
    cout << "R * R^t ?= I"<< endl << R * R.transpose() <<endl;

    Sophus::SO3 SO3_R(R);               // 从旋转矩阵构造Sophus::SO(3)
    Eigen::Quaterniond q(R);            // 从旋转矩阵构造四元数

    // 李代数更新
    Eigen::Vector3d update_so3(0.01, 0.02, 0.03); //更新量
    Sophus::SO3 SO3_updated = SO3_R * Sophus::SO3::exp(update_so3);
    cout<<"SO3 updated = "<< endl << SO3_updated.matrix() <<endl;
    
    //四元数更新
    Eigen::Quaterniond q_update(1, update_so3(0)/2, update_so3(1)/2, update_so3(2)/2); 
    Eigen::Quaterniond q_updated = (q * q_update).normalized(); //四元数归一化
    cout<<"q2R = "<< endl << q_updated.toRotationMatrix() <<endl;

    return 0;
}

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(class1)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加eigen
include_directories("/usr/include/eigen3/")

# 添加sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# 添加可执行程序
add_executable(question2 question2.cpp)
target_link_libraries(question2
    ${Sophus_LIBRARIES}
    )

2. 输出结果
   
   从结果看，通过李代数更新和通过四元数更新对结果的影响不是很大。

5. 求导

推导１：

推导2：

这个和参考的大佬的推得结果略微不同，但结果也应该错不了。