手写VIO第一章作业

发表于 更新于 分类于
我爱VIO，你呢？

VIO入门课程第一章作业

Assignment One

  • Description:

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

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

总结一下：

  1. IMU的频率高，计算速度快，可以弥补视觉在快速运动时丢失的缺陷
  2. 低精度IMU积分定位容易发散，而视觉定位在静止时不会产生漂移
  3. 单目相机无法进行绝对尺度估计，而IMU正好可以

综上所述，IMU和视觉可以达到优势互补，在达到同等定位精度或者鲁棒性的情况下，IMU和视觉的组合可以极大的降低成本，提高性能！

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

常见的VIO融合方案有：

名称 耦合方式 前端 后端与库函数 损失误差函数 回环
MSCKF 紧耦合 FAST+光流 EKF 重投影误差 否
ROVIO 紧耦合 FAST+光流 IEKF 光度(强假设) 否
SVO+MSF 松耦合 FAST EKF Ceres-solver 光度(强假设) 否
VINS 紧耦合 HARRIS+光流 优化g2o 重投影误差 是
VIORB 紧耦合 ORB 优化 重投影误差 是

工业界应用的例子：
工业界里面在AR/VR，自动驾驶，无人机等很多地方都有用到。列举几个著名公司的成果吧，也是我今后比较感兴趣的东西。

  1. Porject Tango of Google：MSCKF
  2. Apple：ARkit
  3. Microsoft：HoloLens

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

学术界VIO发展

  1. 最著名的来自香港科技大学：VINS-Fusion

    • 融合了单双目+IMU+GPS，是目前统一多传感器融合的主流框架
    • 系统时间运行越长，运行延迟越高

  2. 德国慕尼黑工业大学发表：Visual-Inertial Mapping with Non-linear Factor Recovery

    • 该系统是基于BA优化的双目+IMU
    • 在EuRoCs数据集测试效果优于VINS-Fusion
    • 没有开源是硬伤

深度学习方法

  1. Selective Sensor Fusion for Neural Visual-Inertial Odometry
  2. Visual-Inertial Odometry for Unmanned Aerial Vechicle using Deep Learning

Assignment Two

  • Description:

CMakeLists.txt编写

需要用到Eigen、Sophus这两个库，代码如下

1
2
3
4
5
6
7
8
cmake_minimum_required(VERSION 3.0)
project(update_R_or_q)

include_directories("/usr/local/include/Eigen")
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

add_executable(update update_R_or_q.cpp)

主函数update_R_or_q.cpp编写

总体思路如下

  • 创建旋转矩阵和四元数数据

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    cout << "Welcome to the CH1 of VIO lesson!" << endl;
    // define a disturbance of w
    Eigen::Vector3d w(0.01, 0.02, 0.03);
    cout << "The disturbance of w is \n" << w.transpose() << endl;
    // define a rotation matrix
    Eigen::AngleAxisd axis_u(M_PI / 4, Eigen::Vector3d(0, 0, 1));
    Eigen::Matrix3d Rotation;
    Rotation = axis_u.toRotationMatrix();
    cout << "Let's set the rotation matrix is \n" << Rotation << endl;
    Eigen::Quaterniond q(Rotation);
    // normlize (well .... this is unnecessary)
    q.normalize();
    cout << "Let's set the Quaternion is \n" << q.coeffs().transpose() << endl;

  • 利用SO3群对旋转矩阵Rotation进行更新

    1
    2
    3
    4
    5
    6
    7
    8
    9
    // get SO3 group from R or q
    Sophus::SO3d SO3_R(Rotation);
    // Sophus::SO3d SO3_q(q);
    cout << "Rotation matrix from SO3 is \n" << SO3_R.matrix() << endl;
    // cout << "Quaternion vector from SO3 is \n" << SO3_q.matrix() << endl;

    // update SO3_R and SO3_q with the update of w
    Sophus::SO3d SO3_update_R = SO3_R * Sophus::SO3d::exp(w);
    cout << "Updated rotation matrix in SO3 group is \n" << SO3_update_R.matrix() << endl;

  • 利用四元数乘法对Quaternion进行更新

    1
    2
    3
    4
    Eigen::Quaterniond delta_w(1, w(0) / 2, w(1) / 2, w(2) / 2);
    delta_w.normalize();
    Eigen::Quaterniond q_update = q * delta_w;
    cout << "Updated rotation matrix in SO3 group is \n" << q_update.matrix() << endl;

输出结果与分析

两次更新的结果对比

分析对比

上图结果有力地证明了下列公式等价，就不啰嗦啦

R=R⋅exp(ω∧)  ⟺  q=q⊗[112ω]R = R \cdot exp(\omega ^{\wedge}) \iff q = q \otimes \left[\begin{array}{c} 1 \\ \frac{1}{2} \omega \end{array}\right]

Assignment Three

  • Description

推导式(21)

那我就直接开始证明啦！

\begin{equation} \begin{aligned} \frac{d(R^{-1}p)}{dR} & = \underset{\varphi \rightarrow 0}{lim} \frac{(R exp(\varphi^{\wedge}))^{-1} p - R^{-1}p}{\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{exp(\varphi^{\wedge})^{-1}R^{-1} p - R^{-1}p} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{(I-\varphi^{\wedge})R^{-1} p - R^{-1}p} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} - \frac{ \varphi^{\wedge}R^{-1}p} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{(R^{-1} p)^{\wedge} \varphi} {\varphi}\\ & = (R^{-1} p)^{\wedge} \end{aligned} \end{equation}

推导式(22)

前提理论

  • Rotation Matrix为单位正定矩阵

    \begin{equation} \begin{aligned} R^{-1}& = R^T \end{aligned} \end{equation}

  • 一个神秘的公式(推导在VSLAM课程作业里，我已经推到过)

    \begin{equation} \begin{aligned} Rexp(\varphi^{\wedge})R^T & = exp((R\varphi)^{\wedge}) \end{aligned} \end{equation}

  • 李代数表示的逆矩阵可以放到括号里

    • φ\varphi是旋转向量，exp(φ∧)−1exp(\varphi^{\wedge})^{-1} 是反向旋转，所以可以直接变成旋转向量取负

    \begin{equation} \begin{aligned} exp(\varphi^{\wedge})^{-1} & = exp(-\varphi^{\wedge}) \end{aligned} \end{equation}

  • BCH公式展开

    • 其中Jr−1J_r^{-1}代表旋转矩阵的右雅可比

\begin{equation} \begin{aligned} ln(R \cdot exp(\varphi^{\wedge}))^{\vee} &= ln(R)^{\vee} + J_r^{-1}\varphi \end{aligned} \end{equation}

那我也直接开始证明啦！

\begin{equation} \begin{aligned} \frac{dln(R_1R_2^{-1})^{\vee}}{dR_2} & = \underset{\varphi \rightarrow 0}{lim} \frac{ln(R_1 (R_2 exp(\varphi^{\wedge}))^{-1}) ^{\vee} - ln(R_1R_2^{-1})^{\vee}} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{ln(R_1 (R_2^{-1}R_2) exp(\varphi^{\wedge})^{-1}R_2^{-1}) ^{\vee} - ln(R_1R_2^{-1})^{\vee}} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{ln((R_1R_2^{-1}) (R_2 exp(-\varphi^{\wedge})R_2^T)) ^{\vee} - ln(R_1R_2^{-1})^{\vee}} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{ln((R_1R_2^{-1}) exp(-(R_2\varphi)^{\wedge})) ^{\vee} - ln(R_1R_2^{-1})^{\vee}} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{ln(R_1R_2^{-1})-J_r^{-1}(R_2\varphi) - ln(R_1R_2^{-1})^{\vee}} {\varphi}\\ & = \underset{\varphi \rightarrow 0}{lim} \frac{J_r^{-1}(R_2\varphi)} {\varphi}\\ & = J_r^{-1}R_2 \end{aligned} \end{equation}