深蓝学院《从零开始手写VIO》作业1

1.VIO文献阅读

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

PPT中各自优劣：

方案	IMU	视觉
优势	快速响应； 不受成像质量影响； 角速度普遍比较准确； 可估计绝对尺度	不产生漂移； 直接测量旋转与平移
劣势	存在零偏； 低精度 IMU 积分位姿发散； 高精度价格昂贵	受图像遮挡、运动物体干扰; 单目视觉无法测量尺度; 单目纯旋转运动无法估计; 快速运动时易丢失

以下摘自《视觉 SLAM 十四讲》：

1. IMU虽然可以测得角速度和加速度，但这些量都存在明显的漂移（Drift），使得积分两次得到的位姿数据非常不可靠。好比说，我们将IMU放在桌上不动，用它的读数积分得到的位姿也会漂出十万八千里。但是，对于短时间内的快速运动，IMU能够提供一些较好的估计。这正是相机的弱点。当运动过快时，（卷帘快门的）相机会出现运动模糊，或者两帧之间重叠区域太少以至于无法进行特征匹配，所以纯视觉SLAM非常害怕快速的运动。而有了IMU，即370第14讲SLAM：现在与未来使在相机数据无效的那段时间内，我们还能保持一个较好的位姿估计，这是纯视觉SLAM无法做到的。
2. 相比于IMU，相机数据基本不会有漂移。如果相机放在原地固定不动，那么（在静态场景下）视觉SLAM的位姿估计也是固定不动的。所以，相机数据可以有效地估计并修正IMU读数中的漂移，使得在慢速运动后的位姿估计依然有效。
3. 当图像发生变化时，本质上我们没法知道是相机自身发生了运动，还是外界条件发生了变化，所以纯视觉SLAM难以处理动态的障碍物。而IMU能够感受到自己的运动信息，从某种程度上减轻动态物体的影响。
   总而言之，我们看到IMU为快速运动提供了较好的解决方式，而相机又能在慢速运动下解决IMU的漂移问题——在这个意义下，它们二者是互补的。

整体上，视觉和 IMU 定位方案存在一定互补性质：
• IMU 适合计算短时间、快速的运动；
• 视觉适合计算长时间、慢速的运动。
同时，可利用视觉定位信息来估计 IMU 的零偏，减少 IMU 由零偏导致的发散和累积误差；反之， IMU 可以为视觉提供快速运动时的定位

视觉与IMU融合之后会弥补各自的劣势，可利用视觉定位信息来减少IMU由零偏导致的发散和累积误差；IMU可以为视觉提供快速运动时的定位，减少因为外界影响定位失败，同时有效解决单目尺度不可观测的问题

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

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

AR/VR，自动驾驶，无人机，手机、无人机拍照防抖

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

参考知乎：一些基于深度学习的视觉里程计/SLAM开源代码

2. 四元数和李代数更新

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/so3.hpp>

using namespace std;

int main()
{
    Eigen::Vector3d vec(1,2,3);
    cout << "vec :"<< endl << vec.transpose() <<endl<<endl;
    cout << "vecnormd :"<< endl << (vec/vec.norm()).transpose() <<endl<<endl;
    Eigen::AngleAxisd angelaxis(M_PI/4, vec/vec.norm());

    // 待旋转矩阵
    Eigen::Matrix3d R = angelaxis.toRotationMatrix();
    cout << "R :"<< endl << R <<endl<<endl;

    Sophus::SO3d SO3_R(R);               // 从旋转矩阵构造Sophus::SO(3)
    Eigen::Quaterniond q(R);            // 从旋转矩阵构造四元数
    cout << "so3 :"<< endl << SO3_R.log().transpose() <<endl<<endl;
    cout << "q :"<< endl << q.coeffs().transpose() <<endl<<endl;

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

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

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

    cout<<"error = "<< endl <<SO3_updated.matrix()-q_updated.toRotationMatrix()<<endl;
}

3. 其他导数

李群性质：
$$\begin{gathered} \begin{array}{c}\mathbf{C}=\exp \left({\varphi }^{\wedge }\right)\equiv {\sum }_{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n\\ \equiv \cos \varphi 1+(1-\cos \varphi )\mathbf{a}{\mathbf{a}}^T+\sin \varphi {\mathbf{a}}^{\wedge }\\ \approx 1+{\varphi }^{\wedge }\end{array} \\ {\mathbf{C}}^{-1}\equiv {\mathbf{C}}^{\mathrm{T}}\equiv \sum _{n=0}^{\infty }\frac{1}{n!}{\left(-{\mathbf{\varphi }}^{\wedge }\right)}^n\approx 1-{\mathbf{\varphi }}^{\wedge } \end{gathered}$$

$$\begin{array}{rl}\frac{d\left(R^{-1}p\right)}{dR}& =\underset{\varphi \to 0}{\lim }\frac{{\left(R\cdot \exp \left({\varphi }^{\wedge }\right)\right)}^{-1}\cdot p-R^{-1}p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{{\left(\exp \left({\varphi }^{\wedge }\right)\right)}^{-1}{R}^{-1}\cdot p-R^{-1}p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\exp \left(-{\varphi }^{\wedge }\right)\cdot R^{-1}\cdot p-R^{-1}p}{\varphi }\\ & \approx \underset{\varphi \to 0}{\lim }\frac{\left(I-{\varphi }^{\wedge }\right)\cdot R^{-1}\cdot p-R^{-1}\cdot p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{-{\varphi }^{\wedge }{R}^{-1}p}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\varphi \cdot {\left(R^{-1}p\right)}^{\wedge }}{\varphi }\\ & ={\left(R^{-1}p\right)}^{\wedge }\end{array}$$

李群性质：
$$\begin{gathered} (\mathbf{C}\mathbf{u}{)}^{\wedge }\equiv \mathbf{C}{\mathbf{u}}^{\wedge }{\mathbf{C}}^{\mathrm{T}} \\ \exp \left((\mathbf{C}\mathbf{u}{)}^{\wedge }\right)\equiv \mathbf{C}\exp \left({\mathbf{u}}^{\wedge }\right){\mathbf{C}}^{\mathrm{T}} \\ \ln {\left(\exp \left({\mathbf{\varphi }}_1^{\wedge }\right)\exp \left({\mathbf{\varphi }}_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{l}{\mathbf{J}}_l{\left({\mathbf{\varphi }}_2\right)}^{-1}{\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2\text{当}{\mathbf{\varphi }}_1\text{为小量},\\ {\mathbf{J}}_r{\left({\mathbf{\varphi }}_1\right)}^{-1}{\mathbf{\varphi }}_2+{\mathbf{\varphi }}_1\text{当}{\mathbf{\varphi }}_2\text{为小量}.\end{array} \end{gathered}$$

$$\begin{array}{rl}\frac{d\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{d{R}_2}& =\underset{\varphi \to 0}{\lim }\frac{\ln {\left(R_1{\left(R_2\cdot \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\cdot \exp \left(-{\varphi }^{\wedge }\right)\cdot 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\cdot R_2^{-1}\cdot R_2\cdot \exp \left(-{\varphi }^{\wedge }\right)\cdot 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\cdot R_2^{-1}\cdot \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 (\exp {\left(\ln \left({\left(R_1{R}_2^{-1}\right)}^{\vee }\right)\cdot \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{J_r{\left(\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }\right)}^{-1}\cdot \left(-R_2\varphi \right)+\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }-\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }}{\varphi }\\ & =-J_r{\left(\ln {\left(R_1{R}_2^{-1}\right)}^{\vee }\right)}^{-1}\cdot R_2\end{array}$$