VIO 第一讲 概述与课程介绍 {课后作业}

一、课程与内容提要

1、课程的核心内容：视觉与IMU融合定位的基础理论和实现。
2、核心内容具体实现：

• IMU的工作原理和噪声方程
• 视觉与IMU紧耦合的基础理论
• 从零开始实现VIO紧耦合优化器（基于Eigen）

二、VIO概述

参考链接：https://www.cnblogs.com/hitcm/p/6327442.html

三、预备知识回顾

1、三维刚体运动
2、四元数

• 四元数的基本概念和运算
• 四元数和角轴的转换关系：
  
• 旋转的右扰动雅克比与左扰动雅克比：
  
  

四、课后作业

1、VIO文献阅读

1.1 视觉与IMU融合之后的优势？
（1）视觉传感器

• 优点：视觉传感器提供的环境信息丰富，可直接估计出位姿。
• 缺点：视觉传感器是外受型传感器，容易受环境干扰。在快速运动时会出现图像模糊等现象，会影响其位姿估计的精度。当遇到玻璃、白墙等特征稀少的场景时就无法正常工作。

（2）IMU（惯性测量单元）

• 优点：IMU是内受型传感器，不受外界环境的影响，其次，IMU可以提供几百到几千HZ的频率输出，相对于几十HZ的相机而言，IMU对快速运动的响应会更好。
• 缺点：IMU估计位姿是通过连续的积分实现的，长时间的积分漂移会很大，从而造成位姿估计不准确。

（3）视觉与IMU融合的优势

• IMU可以为单目相机提供尺度信息。从机器人的运动方程出发，以IMU作为预测方程，以视觉作为观测方程的状态估计方程，相对于只是使用视觉的重投影方式提供了更多的信息约束。有利于提高位姿估计的精度和提高系统在高速运动以及纯旋转的条件下的鲁棒性。其次，从器件本身的属性和经济成本考虑，VIO算法是空间定位实现的标准方案。

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

（2）工业界：苹果的ARKit，谷歌的Tango、ARcore等。

1.3 在学术界，VIO研究有哪些新的进展？有没有将学习方法用到VIO中的例子？
（1）学术界：VINS、VINS-Fusion、 OKVIS等。

• 2、四元数和李代数更新

主函数：main.cpp

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>



using namespace std;
/* 1、定义旋转矩阵R 及 四元素q
 * 2、利用计算出来的小量w对R q更新.小量为w=[0.01,0.02,0.03]T
 * R <- R*exp(w^)
 * q <- q*[1,1/2*w]T
*/


int main() {
    // 设置R q,假设初始旋转为绕z轴旋转90度
    //Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
    //cout<<R<<endl;
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4,Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Eigen::Quaterniond q = Eigen::Quaterniond(R);
    cout << "R is = " << endl << R << endl;
    cout << "q is = " << q.coeffs().transpose() << endl; // 实部在后，虚部在前

    // w是计算出来的增量.轴角形式=(v，theta),v为单位向量,theta是向量的模
    Eigen::Vector3d w (0.01,0.02,0.03);

    //w的模=m
    double m = sqrt(w(0)*w(0)+w(1)*w(1)+w(2)*w(2));

    // 旋转向量转换成旋转矩阵
    // 此处等价于【w的指数映射】【罗德里格公式（旋转向量->旋转矩阵）】
    Eigen::Matrix3d w_ = Eigen::AngleAxisd(m,Eigen::Vector3d(0.01/m,0.02/m,0.03/m)).toRotationMatrix();
    /**************** 此处用指数映射计算(不推介但可以用)： *************/
     【注】Eigen里边的exp()函数是对每个元素求exp，不可用在此处，此处通过matlab的expm函数算出w_hat的指数映射
    Eigen::Matrix3d w_hat;
    w_hat << 0, -w(2), w(1),
            w(2), 0, -w(0),
            -w(1), w(0), 0;
    Eigen::Matrix3d w_hat_exp;
    w_hat_exp <<0.9994,   -0.0299,    0.0201,
                0.0301,    0.9995,   -0.0097,
               -0.0198,    0.0103,    0.9998;
    Eigen::Matrix3d R_exp = R*w_hat_exp;
    cout<<"R updated by exp is:"<<endl<<R_exp<<endl;
    // 使用R方式存储，更新R
    R = R * w_;
    cout << endl <<  "R_update with Rodrigues's Formula is =" << endl << R << endl;

    // 使用q方式储存
    Eigen::Quaterniond q_ = Eigen::Quaterniond(1, 0.5*w(0), 0.5*w(1), 0.5*w(2));
    // 更新q
    q = q * q_;
   // cout << "q is = " << q.coeffs().transpose() << endl;
    q.normalize(); // 归一化
  //  cout << "q is = " << q.coeffs().transpose() << endl;
    // 四元数转化为旋转矩阵
    Eigen::Matrix3d q2R = q.toRotationMatrix();
    cout << endl << "R_update form q_update is = " << endl << q2R << endl;

    // 作差比较精度
    Eigen::Matrix3d diff_exp = R_exp - q2R;
    cout << endl << "diff_ between R & q with exp is = " << endl << diff_exp << endl;

    Eigen::Matrix3d diff_rod = R - q2R;
    cout << endl << "diff_ between R & q with Rodrigues's Formula is = " << endl << diff_rod << endl;
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.5.1)
project(question2)

include_directories("/usr/include/eigen3")

set(CMAKE_CXX_STANDARD 11)
add_executable(question2 main.cpp)

结论：

R is = 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
q is =        0        0 0.382683  0.92388
R updated by exp is:
  0.685399  -0.727896  0.0210718
  0.727966   0.685611 0.00735391
   -0.0198     0.0103     0.9998

R_update with Rodrigues's Formula is =
  0.685368  -0.727891  0.0211022
  0.727926   0.685616 0.00738758
-0.0198454  0.0102976    0.99975

R_update form q_update is = 
  0.685371  -0.727888  0.0210998
  0.727924   0.685618 0.00738668
-0.0198431  0.0102964    0.99975

diff_ between R & q with exp is = 
 2.77478e-05 -7.27358e-06 -2.79704e-05
 4.26413e-05 -7.52098e-06   -3.277e-05
 4.30549e-05  3.60374e-06  4.99125e-05

diff_ between R & q with Rodrigues's Formula is = 
 -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

• 3、其他导数