手写VIO --学习笔记 - Part1

一、VIO文献阅读

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

1、视觉与IMU进行融合之后有何优势？
IMU：(Inertial Measurement Unit)，惯性测量单元

典型6轴 IMU 以较高频率（$\ge 100Hz$）返回被测量物体的角速度与加速度
短时间内，IMU的精度很高
受自身温度、零偏、振动等因素干扰，积分得到的平移和旋转容易漂移

视觉：(Visual Odometry)

以图像形式记录数据，频率较低（$15-60Hz$ 居多）
通过图像特征点或像素推断相机运动
如果遇到白墙，或者纯色的一些特殊场景，基本上无法提取特征

二者优势互补：

可以借助IMU较高的采样频率，进而提高系统的输出频率。
可以提高视觉的鲁棒性，如视觉SLAM因为某些运动或场景出现的错误结果。
可以有效的消除IMU的积分漂移。
能够校正IMU的Bias。
单目与IMU的融合可以有效解决单目尺度不可观测的问题。
可以应对快速的运动变化，相机在快速运动过程中会出现运动模糊。

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

常见的融合方案：

VINS (单目+IMU、双目+IMU)
OKVIS (单目+IMU、双目+IMU)
ROVIO (单目+IMU）
RKSLAM(单目+IMU）
ORB-SLAM3（双目+IMU）

工业界应用

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测量并生成估计轨迹，然后根据相对于像素坐标的空间网格的缩放图像投影误差的雅可比行列式在线校正。

参考知乎：一些基于深度学习的视觉里程计/SLAM开源代码
深度学习SLAM ：最新的基于深度学习的deepvo，VINet，大家怎样评价？

二、四元数和李代数更新

课件提到了可以使用四元数或旋转矩阵存储旋转变量。当我们用计算出来的$\omega$对某旋转更新时，有两种不同方式:
$$R⟵Rexp({\omega }^{\wedge })$$
或$$q⟵q[1,\frac{1}{2}{\omega }^{\wedge }]$$

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

参照《VSLAM14讲》p48和p87页代码中对于eigen库和sophus库的使用。

以（1，2，3）为转轴， 角度45度

//
// Created by daybeha on 2022/2/11.
//

#include 
#include 
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main(){
    // 注意第二个参数是转轴，需要归一化
    Matrix3d R = AngleAxisd(M_PI/4, Vector3d(1, 2, 3).normalized()).toRotationMatrix();
    Quaterniond q(R);
//    Sophus::SO3d SO3_R(R);      两种构造方式
    Sophus::SO3d SO3_R(R);
    cout << "\nmatrix from SO(3):\n" << SO3_R.matrix()<< endl;
    cout << "matrix from Quaternion:\n" << q.matrix() << endl;

    /// 增量扰动模型的更新
    // SO3更新
    Vector3d w(0.01, 0.02, 0.03);
    Sophus::SO3d SO3_updated = SO3_R * Sophus::SO3d::exp(w);
    cout << "\nSO3 updated = \n" << SO3_updated.matrix() <<endl;
    // 四元数更新
    Quaterniond q_updated = q * Quaterniond(1, w[0]/2, w[1]/2, w[2]/2).normalized();
    cout << "q updated = \n" << q_updated.matrix() << endl;

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

    return 0;
}

运行结果：

可以看到，两种不同方式得到的更新结果差别很小，但不是没有……

三、其他导数

使用右乘$\mathfrak{s}\mathfrak{o}(3)$，推导以下导数：
$$\frac{d{R}^{-1}p}{dR}$$
答：

设扰动$△R$对应的李代数为$\phi$， 则有右扰动:
$$\begin{array}{rl}\frac{\partial R^{-1}p}{\partial R}& =\frac{\partial (Rexp({\phi }^{\wedge }){)}^{-1}p-R^{-1}p}{\partial \phi }\\ & =\frac{\partial (exp({\phi }^{\wedge }{)}^{-1}{R}^{-1}p-R^{-1}p}{\partial \phi }\\ & =\frac{\partial (exp(-{\phi }^{\wedge })R^{-1}p-R^{-1}p}{\partial \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 }\\ & =\underset{\phi \to 0}{\lim }\frac{(R^{-1}p{)}^{\wedge }\phi }{\phi }\\ & =-(R^{-1}p{)}^{\wedge }\end{array}$$

$$\frac{dln(R_1{R}_2^{-1})}{d{R}_2}$$
答：

由BCH的线性表达
$$\begin{array}{r}ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2,当{\varphi }_1为小量\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1,当{\varphi }_2为小量\end{array}\end{array}$$

其中

$\varphi =\theta a,a为单位向量$
$J_l=J_{VSLA{M}_{P}80}=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }$
$J_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge }$
$J_r(\varphi )=J_l(-\varphi )$
$J_r(\varphi {)}^{-1}=J_l(-\varphi {)}^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T+\frac{\theta }{2}{a}^{\wedge }$

另外还用到$so(3)$的伴随性质：

$$Rexp(p^{\wedge })R^T=exp((Rp{)}^{\wedge })\text{\hspace{1em}(1)}$$

---

同样设扰动$△R1$对应的李代数为$\phi$， 则有右扰动
$$\begin{array}{rl}\frac{\partial ln(R_1{R}_2^{-1}{)}^{\vee }}{\partial R_2}& =\underset{\phi \to 0}{\lim }\frac{ln(R_1(R_{2}exp({\phi }^{\wedge }){)}^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_{1}exp({\phi }^{\wedge }{)}^{-1}{R}_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}{R}_{2}exp({\phi }^{\wedge }{)}^{-1}{R}_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}{R}_{2}exp(-{\phi }^{\wedge })R_2^T{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ 公式(1)& =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^{-1}exp((-R_2\phi {)}^{\wedge }){)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ (BCH)& \approx \underset{\phi \to 0}{\lim }\frac{J_r(ln(R_1{R}_2^{-1}{)}^{\vee }{)}^{-1}(-R_2\phi )+ln(R_1{R}_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-J_r(ln(R_1{R}_2^{-1}{)}^{\vee }{)}^{-1}{R}_2\phi }{\phi }\\ & =-J_r(ln(R_1{R}_2^{-1}{)}^{\vee }{)}^{-1}{R}_2\end{array}$$

笔记 - 关于为什么要用右乘：

过程中很多内容需要在世界坐标系下讨论，因此需要使用$T_{WI}$表示IMU位姿；而要使用$T_{WI}$，再计算世界坐标时就需要右乘。

参考

深蓝学院-手写VIO作业-第一章
深蓝学院《从零开始手写VIO》作业1
深蓝学院《从零开始手写VIO》作业一
从零手写VIO——（一）Introduction
https://gitee.com/ximing689/vio-learning.git