【深蓝学院】手写VIO第1章

0. 引言

完成了SLAM14讲的学习，来VIO进行进阶。

1. 概述与课程介绍

Section1. 课程介绍

Section2. VIO概述

为什么把视觉和IMU结合在一起？
因为这两个sensor是互补的，视觉测慢的，IMU测快的；视觉漂移小，IMU有漂移。

说VIO要明确是跟什么层次的IMU进行融合，各种层次的IMU的角速度普遍比较准确，但便宜的IMU加速度计精度差，基本上只能看个方向，贵的好的可以直接通过积分得到位姿。在飞机上用的很好的IMU，同样的算法在手机上可能就没那么好了。

手机的IMU可能几s就飞了，汽车级别的IMU可能估计个30s就飞了（在GPS，Lidar等失效的情况下）

VIO是在工业界用的较多的一种方案，主要在AR/VR， Robotic/无人机(drone)，等，这些场景Lidar的功耗太高，不适合使用，比如VR眼镜，装个IMU是可以的。

而且在场景中要分清楚到底是基于地图的定位还是基于odometer的定位。基于odometer的定位是只要知道他的相对运动，而如果需要知道相对于真实物理世界的运动，则需要建图等，在地图中定位。

需要指出，相对运动无论多准都会有累积误差(VIO是估计相对位姿的)，即使VIO很准还是会飘，就变成原来15s不能用，现在30s不能用的东西了。如果是基于地图定位就不用太过纠结odometry，AR/VR需要快速地知道短时间内物体的运动，VIO可用，手机防抖

视觉可以纠正IMU的零偏，IMU可以为视觉提供尺度信息，或者在视觉丢失的时候顶上去。

VIO可分为松耦合和紧耦合。

• 松耦合： IMU和视觉自己算自己的，通过后处理的方法将两个结果融合（典型融合方法是Kalman滤波）。（因为后处理不会影响前面的结果，所以二者是相互独立的。）
  

• 紧耦合：IMU和视觉相互弥补。（视觉可能是一个BA，IMU是一组运动方程 ）课程主要介绍紧耦合。
  

GPS，RTK受场景约束，有些场景很好用，有些场景就不行。

Section 3. 预备知识回顾

默认以$Twi$表示IMU的定位信息，平移可以直接看作IMU在world中的坐标。

由四元数的叉乘可得，一个四元数$q$等于自身与$[0,1{]}^T$做叉乘。

四元数求导：

对cos和sin进行泰勒展开即得四元数的变化量。

两个四元数叉乘就代表把左边的四元数按照右边的旋转转了一下。
参考：

$\delta \theta$当$△t$趋近于0时即为角速度$\omega$

当使用旋转矩阵表示旋转时（和四元数没有本质上的区别），导数可以用泊松公式表示：

因为$a^{\wedge }b=a\times b$，所以也可以写成${\mathbf{\omega }}_{\times }$

左乘时$\frac{\partial Rp}{\partial \varphi }=-(Rp{)}^{\wedge }$，右乘是$-R{p}^{\wedge }$
导数算出来后，如果是按照左扰动模型求的，就左乘更新上去，右扰动就右乘更新上去。

SO(3)导数，比如对旋转的导数

本课程习惯使用右乘

3.作业

1.T1

读完了这篇论文，感觉一般。

1.两个sensor是互补的，视觉测慢的，IMU测快的；视觉漂移小，IMU有漂移。
来自作业分享：

2.VINS Mono等
工业应用：AR/VR，Robotics/无人机(drone)，手机防抖，这些场景Lidar功耗太高，不适合， IMU较为合适。

2.T2

照着公式写代码即可。
第一种是李代数so(3)指数映射，第二种是四元数更新，小量$\theta$相当于$\omega$，刚好用到上面说的四元数相乘，把左边的按照右边旋转转了一下

#include 
#include 
#include 
#include "sophus/so3.hpp"

//编程验证四元数和旋转矩阵的旋转是相同的

using namespace Eigen;
using namespace std;

int main() {
    std::cout << "Hello, World!" << std::endl;


    Vector3d w(0.01, 0.02, 0.03);  //这是so3

    AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
    Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();
    Sophus::SO3d SO3_R(rotation_matrix);
    cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
    Sophus::SO3d SO3_updated = SO3_R * Sophus::SO3d::exp(w);  //核心1（指数映射）
    cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
    cout << "SO3_updated q:\n" << Quaterniond(SO3_updated.matrix()).coeffs().transpose() << endl;
    

    Quaterniond q = Quaterniond(rotation_vector);
    Quaterniond wq = Quaterniond(1,0.5*w(0),0.5*w(1), 0.5*w(2));  //核心2（四元数更新，小量theta相当于w）
    cout << "quaternion from rotation vector = " << q.coeffs().transpose()
         << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    q = q * wq;
    cout << "QUaternion updated q: " << q.coeffs().transpose() << endl;

    return 0;
}

作业分享：

3.T3

第一个，手写了一下：

第二个稍微复杂了一点点：

第1章完。