《视觉SLAM进阶：从零开始手写VIO》第一讲作业

《视觉SLAM进阶：从零开始手写VIO》第一讲作业

1.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可以为视觉提供快速运动时的定位，以及因为某种因素（场景特征点较少，光照变化较大等）定位失败时。

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

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

• MSCKF
• OKVIS
• ROVIO
• VIORB
• VINS-Mono，VINS-Mobile，VINS-Fusion

以上方案基本都在Github上开源了。

a) 常见的视觉 +IMU 融合方案

名称	耦合方式	后端及库	前端	视觉误差	是否回环
MSCKF	紧耦合	EKF	Fast+ 光流	重投影	否
ROVIO	紧耦合	IEKF	Fast+ 光流	光度	否
SVO+MSF	松耦合	EKF Ceres	FAST	光度	否
VINS	紧耦合	优化 g2o	Harris+ 光流	重投影	是
VIORB	紧耦合	优化	ORB	重投影	是
OKVIS	紧耦合	优化 Ceres	Harris+BRISK	重投影	否
SVO+iSAM2		优化 GTSAM		光度	否

（2）工业界应用

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

b) 工业界应用的例子
i. 谷歌 Project Tango： MSCKF
ii. 苹果的 ARkit
iii. 微软的 HoloLens： IMU 感应 HoloLens 的方向，环境感知摄像头感应 HoloLens 相对位置的偏移，深度摄像头感知 HoloLens 周围环境。 KinectFusion+IMU+ 单目 VO

1.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.

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

a.VIO 结合直线和点特征

​ 如：2018 rifo-VIO: Robust and Efficient Stereo Visual Inertial Odometry using Points and Lines

​ *Stereo Visual-Inertial SLAM With Points and Lines*

b.时间在线标定

​ 如：Online Temporal Calibration for Monocular Visual-Inertial Systems
c.多机器人协作

​ 如CVI-SLAM – Collaborative Visual-Inertial SLAM
d.利用深度学习实现深度估计（机器学习）

​ 如; Embedding Temporally Consistent Depth Recovery for Real-time Dense Mapping in Visual-inertial Odometry

e. 港科大多传感器融合

​ A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors

2 四元数和李代数更新

由于 ϕ 是三维向量，我们可以定义它的模长和它的方向，分别记作 θ 和 a，于是有 ϕ = θa。这里 a 是一个长度为 1 的方向向量，即 ∥a∥ = 1。a是单位向量

---

//利用Rodrigues’s formula完成旋转矩阵更新

ω=θa

求ω的模长θ

求ω的单位向量a

求ω的a^

求 罗德里格斯公式 exp(ω^)=cosθ*I + (1 + cosθ)aa^T +sinθ *a^

求 更新量 R=R*exp(ω^)

---

//四元数更新

求
$$[1,1/2w]$$
求
$$q\ast [1,1/2w]$$
求 更新后的单位四元数 单位四元数可表达任意三维旋转，且无奇异性。

---

代码实现一：

#include <iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
using namespace std;

int main(int argc, char **argv) {
    //变量初始化
    Eigen::Vector3d w(0.01,0.02,0.03);//小量角速度（旋转向量）
    Eigen::Matrix3d R=Eigen::AngleAxisd(M_PI/4,Eigen::Vector3d(0,0,1)).toRotationMatrix();//初始旋转矩阵，绕Z轴旋转45°
    Eigen::Quaterniond q(R);//初始四元数
    // Eigen::Quaterniond q = Eigen::Quaterniond(R);
    cout<<"初始旋转矩阵："<<endl;
    cout<<R<<endl;
    cout<<"初始四元数："<<endl;
    cout<<q.coeffs().transpose() <<endl;//注意输出四元数的基本格式
    
    //利用Rodrigues's formula完成旋转矩阵更新
    double theta=w.norm();//旋转向量对应的模长
    // double  theta = sqrt( w(0)*w(0)+w(1)*w(1)+w(2)*w(2) );
    Eigen::Vector3d n_w=w/theta;//归一化得到旋转向量对应的旋转轴  单位向量
    Eigen::Matrix3d n_w_skew;
    n_w_skew<<   0,    -n_w(2),    n_w(1),
		n_w(2),     0,     -n_w(0),
	       -n_w(1),  n_w(0),      0;
    Eigen::Matrix3d R_w=cos(theta)*Eigen::Matrix3d::Identity()+(1-cos(theta))*n_w*n_w.transpose()+sin(theta)*n_w_skew;//Rodrigues's formula
    Eigen::Matrix3d R_update=R*R_w;
    cout<<"更新后的旋转矩阵："<<endl;
    cout<<R_update<<endl;    
    
    //四元数更新    
    Eigen::Quaterniond q_w(1,w(0)/2,w(1)/2,w(2)/2);//小量角速度对应的四元数
    Eigen::Quaterniond q_update=q*q_w;
    q_update=q_update.normalized();//单位四元数才可以表示三维旋转，所以必须归一化
    cout<<"更新后的四元数："<<endl;
    cout<<q_update.coeffs().transpose() <<endl; //注意输出四元数的基本格式 
    
    //计算两种方法得到的结果之差
    cout<<"两种方法得到的结果之差："<<endl;    
    cout<<q_update.toRotationMatrix()-R_update<<endl;

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(CH1)
include_directories("/usr/include/eigen3")
add_executable(CH1 main.cpp)

**运行结果如下：**运行 命令g++ Eigen.cpp -o Eigen；再运行./Eigen

初始旋转矩阵：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
初始四元数：
       0        0 0.382683  0.92388
更新后的旋转矩阵：
  0.685368  -0.727891  0.0211022
  0.727926   0.685616 0.00738758
-0.0198454  0.0102976    0.99975
更新后的四元数：
0.000792425 0.0111503  0.396472  0.917979
两种方法得到的结果之差：
  2.5963e-06  2.37368e-06 -2.44789e-06
-2.38192e-06  2.53859e-06 -8.98416e-07
 2.29623e-06 -1.23557e-06  5.83041e-08

可以看出两种更新方式差异很小，差异量级约为 10-6。

代码实现二：

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
/* 1、定义R 及 q
 * 2、利用计算出来的w对R q更新.增量为w=[0.01,0.02,0.03]T
 * R <- R*exp(w^)
 * q <- q*[1,1/2*w]T
*/

int main()
{
    // 设置R q,假设初始旋转为绕z轴旋转45度
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Eigen::Quaterniond q = Eigen::Quaterniond(R);

    cout << "R is = " << endl << R << endl;
    cout << "q is = " << q.coeffs().transpose() << endl; // 实部在后，虚部在前

    // w是计算出来的增量.轴角形式=(v，theta),v为单位向量,theta是向量的模
    Eigen::Vector3d w(0.01, 0.02, 0.03);

    // w的模=m
    double m = sqrt( w(0)*w(0)+w(1)*w(1)+w(2)*w(2) );
    // 旋转向量转换成旋转矩阵
    // 此处等价于【w的指数映射】【罗德里格公式（旋转向量->旋转矩阵）】
    Eigen::Matrix3d w_ = Eigen::AngleAxisd( m, Eigen::Vector3d(0.01/m, 0.02/m, 0.03/m) ).toRotationMatrix();

    /**************** 此处用指数映射计算(不推介但可以用)： *************/
    // 【注】Eigen里边的exp()函数是对每个元素求exp，不可用在此处，此处通过matlab的expm函数算出w_hat的指数映射
    Eigen::Matrix3d w_hat;
    w_hat << 0, -w(2), w(1),
            w(2), 0, -w(0),
            -w(1), w(0), 0;
    Eigen::Matrix3d w_hat_exp;
    w_hat_exp <<    0.9994,   -0.0299,    0.0201,
                    0.0301,    0.9995,   -0.0097,
                    -0.0198,    0.0103,    0.9998;

    Eigen::Matrix3d R_exp = R * w_hat_exp;
    cout << "R_update with exp is =" << endl << R_exp << endl;
    /************************************************************/

    // 使用R方式存储，更新R
    R = R * w_;
    cout << endl <<  "R_update with Rodrigues's Formula is =" << endl << R << endl;

    // 使用q方式储存
    Eigen::Quaterniond q_ = Eigen::Quaterniond (1, 0.5*w(0), 0.5*w(1), 0.5*w(2));
    // 更新q
    q = q * q_;
    q.normalize(); // 归一化

    // 四元数转化为旋转矩阵
    Eigen::Matrix3d q2R = q.toRotationMatrix();
    cout << endl << "R_update form q_update is = " << endl << q2R << endl;

    // 作差比较精度
    Eigen::Matrix3d diff_exp = R_exp - q2R;
    cout << endl << "diff_ between R & q with exp is = " << endl << diff_exp << endl;

    Eigen::Matrix3d diff_rod = R - q2R;
    cout << endl << "diff_ between R & q with Rodrigues's Formula is = " << endl << diff_rod << endl;
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.12)
project(ch1)

set(CMAKE_CXX_STANDARD 11)

include_directories( "/usr/include/eigen3" )
add_executable(ch1 Eigen.cpp)

运行结果:

R is = 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
q is =        0        0 0.382683  0.92388
R_update with exp is =
  0.685399  -0.727896  0.0210718
  0.727966   0.685611 0.00735391
   -0.0198     0.0103     0.9998

R_update with Rodrigues's Formula is =
  0.685368  -0.727891  0.0211022
  0.727926   0.685616 0.00738758
-0.0198454  0.0102976    0.99975

R_update form q_update is = 
  0.685371  -0.727888  0.0210998
  0.727924   0.685618 0.00738668
-0.0198431  0.0102964    0.99975

diff_ between R & q with exp is = 
 2.77478e-05 -7.27358e-06 -2.79704e-05
 4.26413e-05 -7.52098e-06   -3.277e-05
 4.30549e-05  3.60374e-06  4.99125e-05

diff_ between R & q with Rodrigues's Formula is = 
 -2.5963e-06 -2.37368e-06  2.44789e-06
 2.38192e-06 -2.53859e-06  8.98416e-07
-2.29623e-06  1.23557e-06 -5.83041e-08++

3 其他导数

用到的性质
$$\begin{array}{rl}& R^{-1}=R^T\\ & \ln (\mathrm{R}\exp \left({\varphi }^{\wedge }\right)=\ln (R)+J_r^{-1}\varphi \end{array}$$

$$\begin{gathered} SO3\text{上的伴随性质} \\ {R}^T\exp \left({\varphi }^{\wedge }\right)R=\exp ({\left(R^T\varphi \right)}^{\wedge }) \\ R\exp \left(p^{\wedge }\right)R=\exp ({\left(Rp\right)}^{\wedge }) \end{gathered}$$

SO(3) 的伴随性质：具体证明过程参考https://blog.csdn.net/qq_17032807/article/details/84942548

使用右乘推导以下导数 ：
$$\begin{array}{rl}& \frac{\mathrm{d}\left(R^{-1}p\right)}{\mathrm{d}R}\\ & =\underset{\varphi \to 0}{\lim }\frac{{\left(\mathrm{R}\exp \left({\varphi }^{\wedge }\right)\right)}^{-1}p-R^{-1}p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\exp {\left({\varphi }^{\wedge }\right)}^{-1}{R}^{-1}p-R^{-1}p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\left(I-{\varphi }^{\wedge }\right)R^{-1}p-R^{-1}p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\left(-{\varphi }^{\wedge }\right)R^{-1}p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{{\left(R^{-1}p\right)}^{\wedge }\varphi }{\varphi }\\ & ={\left(R^{-1}p\right)}^{\wedge }\end{array}$$

---

$$\begin{array}{rl}& \frac{\mathrm{d}\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\mathrm{d}{R}_2}\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left(R_1{\left(R_2\exp \left({\varphi }^{\wedge }\right)\right)}^{-1}\right)}^{\vee }-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left(R_1\exp {\left({\varphi }^{\wedge }\right)}^{-1}{R}_2^{-1}\right)}^{\vee }-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left(R_1{R}_2^{-1}\left(R_2\exp {\left({\varphi }^{\wedge }\right)}^{-1}{R}_2^{-1}\right)\right)}^{\vee }-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left(R_1{R}_2^{-1}\left(R_2\exp \left(-{\varphi }^{\wedge }\right)R_2^{-1}\right)\right)}^{\vee }-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left(R_1{R}_2^{-1}\exp \left({\left(R_2(-\varphi )\right)}^{\wedge }\right)\right)}^{\vee }-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }-J_r^{-1}{R}_2\varphi -\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =-J_r^{-1}\left(\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }\right)R_2\end{array}$$

---

$$\begin{array}{rl}& \frac{\mathrm{d}\ln \left(R_1{R}_2^{-1}\right)}{\mathrm{d}{R}_2}\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left(R_1{\left(R_2\exp \left({\varphi }^{\wedge }\right)\right)}^{-1}\right)-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left(R_1\exp {\left({\varphi }^{\wedge }\right)}^{-1}{R}_2^{-1}\right)-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left(R_1{R}_2^{-1}\left(R_2\exp {\left({\varphi }^{\wedge }\right)}^{-1}{R}_2^{-1}\right)\right)-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left(R_1{R}_2^{-1}\left(R_2\exp \left(-{\varphi }^{\wedge }\right)R_2^{-1}\right)\right)-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left(R_1{R}_2^{-1}\exp \left({\left(R_2(-\varphi )\right)}^{\wedge }\right)\right)-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln \left(R_1{R}_2^{-1}\right)-J_r^{-1}{R}_2\varphi -\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =-J_r^{-1}\left(\ln \left(R_1{R}_2^{-1}\right)\right)R_2\end{array}$$