【vio综述一】

1. 视觉与IMU之间融合的优势
2. 常见的视觉与IMU融合的例子，以及落地相关的工业界应用例子
3. 进行文献调研，了解学术界VIO的进展，以及机器学习在VIO中的应用
4. 编程实现四元素和李代数的更新
5. 求导

一、视觉与IMU之间融合的优势

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

1. IMU可以测得物体角速度和加速度，但是长时间的运动存在不可忽略的漂移，两次积分求得的位移出现误差不断积累，并不断传播。相机在快速运动以及短时间内的效果很差。跟相机的频率有关。
2. 相机的数据不会出现漂移，可以修正IMU的漂移。
3. 针对动态物体，IMU感知自身运动，不会出现相对运动的情况。

总之，IMU喜欢快，短时间的运动，但是有误差，相机误差小，喜欢长时间的运动物体，但是害怕运动较快的物体。

同时，可利用视觉定位信息来估计 IMU 的零偏，减少 IMU 由零偏导致的发散和累积误差；反之， IMU 可以为视觉提供快速运动时的定位

视觉与IMU融合之后会弥补各自的劣势，可利用视觉定位信息来减少IMU由零偏导致的发散和累积误差；

IMU可以为视觉提供快速运动时的定位，减少因为外界影响定位失败，同时有效解决单目尺度不可观测的问题。

二、常见的视觉与IMU融合的例子，以及落地相关的工业界应用例子

VINS (单目+IMU、双目+IMU)，优化的方法
OKVIS (单目+IMU、双目+IMU)
ROVIO (单目+IMU）滤波
MSCKF 滤波的方案
RKSLAM (单目+IMU）
ORB_SLAM 系列（单目+IMU）
AR/VR，自动驾驶，无人机，手机、无人机拍照防抖

可以参考ORB—SLAM3的表1格中的数据
常见大厂均有使用案例

三、进行文献调研，了解学术界VIO的进展，以及机器学习在VIO中的应用

https://zhuanlan.zhihu.com/p/139150194?utm_source=wechat_session&utm_medium=social&utm_oi=697411568186429440

四、编程实现四元素和李代数的更新

单位四元数或者旋转矩阵可以存储旋转变量，计算对应的w对某一个旋转更新时，有两种方法进行更新： $$\begin{gathered} R\leftarrow Rexp(w^{\wedge }) \\ q\leftarrow q\otimes [1,\frac{1}{2}w{]}^T \end{gathered}$$

对于小量w,如下所示 更新，比较两种量的更新之后的差别，可得出结论，不论哪一种的更新方式，效果等同，不加区分 $$w=[0.01,\mathrm{0.02.0.03}{]}^T$$

作业如下：

介绍罗德里戈公式：

一个向量经过旋转矩阵旋转之后得到一个新的向量。

已知，k是旋转轴的单位向量，v 是任意向量， 角度theata是旋转角度，表示如下：
$$v_{rot}=vcos\theta +(1-cos\theta )(vk)k+sin(\theta )\times k$$

对应的有：
$$v_{rot}=Rv$$

$$\left[\begin{array}{c}{v}_x^{\prime }\\ v_y^{\prime }\\ v_z^{\prime }\end{array}\right]=R\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$$

$$R=Ecos(\theta )+(1-cos(\theta ))\left[\begin{array}{c}{k}_x\\ k_y\\ k_z\end{array}\right][k_x,k_y,k_z]+sin(\theta )\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ k_y& k_x& 0\end{array}\right]$$

代码基本逻辑框图：

编写代码如下：

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

int main() {
    //给定旋转轴和角度对应的旋转矩阵
    //给定旋转更新小量
    //使用罗德里戈公式进行更新
    //使用四元数进行更新
    //对比二者的误差
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
    Eigen::Quaterniond q(R);

    Eigen::Vector3d w(0.01, 0.02, 0.03);

    cout << "给定的原始转换的旋转矩阵和无穷小量的旋转的扰动" << endl;
    cout << R << endl;
    //注意输出四元数的基本格式
    cout << q.coeffs().transpose() << endl;

    //更新方式一，使用罗德里戈公式进行更新。
    //旋转小量对应的小扰动为，求对应的轴和角度。theta可以认为是旋转角度
    //返回是二范数，弧度制
    double theta = w.norm();
    //归一化更新旋转轴
    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 =
            Eigen::Matrix3d::Identity() * cos(theta) + (1 - cos(theta)) * n_w * n_w.transpose() + sin(theta) * n_w_skew;
    Eigen::Matrix3d R_update = R * R_W;
    //实部设为1??目前也只能设为1
    Eigen::Quaterniond q_w(1, 0.5 * w(0), 0.5 * w(1), 0.5 * w(2));
    //务必进行旋转归一化的操作，原因是仅有单位四元数表示一个旋转
    Eigen::Quaterniond q_update = (q * q_w).normalized();

    Eigen::Matrix3d diff = q_update.toRotationMatrix() - R_update;
    cout << diff << endl;

    std::cout << "Hello,wpf！" << std::endl;
    return 0;
}

对应的CMmakeLists.txt文件如下：

cmake_minimum_required(VERSION 3.20)
project(ch1)
include_directories("usr/include/eigen3")
set(CMAKE_CXX_STANDARD 14)
add_executable(ch1 main.cpp)

对应实验结果如下：

/home/wpf/SLAM/vio/vio_works/ch1/cmake-build-debug/ch1 给定的原始转换的旋转矩阵和无穷小量的
0.707107 -0.707107 0
0.707107 0.707107 0 0 0 1 0 0 0.382683 0.92388
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 Hello,wpf！

五. 求导

从略