VIO概述

1. VIO整体概述

在这里插入图片描述

松耦合：各部分自己算自己的，最后单独把数据来算
紧耦合：同时考虑这两个问题（效果更好）
IMU+GPS精度可以达到cm级，但是受环境影响比较大

融合方案
在这里插入图片描述采用卡尔曼滤波，当一边数据来源不准确时，整体受影响

在这里插入图片描述

2. 预备数学知识

2.1 三维刚体运动

在这里插入图片描述

2.2 四元数

在这里插入图片描述
对时间求导

在这里插入图片描述

2.3 李代数

在这里插入图片描述

这个课程就没有怎么用 $SE (3)$ , 直接用 $SO (3) + t$

3. 作业

在这里插入图片描述

test.cpp

#include <iostream>
#include <ctime>#include <Eigen/Core>
#include <Eigen/Dense>#include <sophus/so3.h>
#include <sophus/se3.h>using namespace std;int main()
{Eigen::Vector3d w(0.01,0.02,0.03);Eigen::Matrix3d R_I = Eigen::Matrix3d::Identity();Sophus::SO3 SO3_R_I(R_I);Eigen::Quaterniond q_I = Eigen::Quaterniond(R_I);// 输出更新之前的R和qcout << endl << "before update, R is: " << endl << SO3_R_I.matrix() << endl;cout << endl << "before update, q is: " << q_I.coeffs().transpose()<< endl;//updatecout << "update ..." << endl;Sophus::SO3 SO3_updated = SO3_R_I * Sophus::SO3::exp(w); //对R 进行右乘更新cout << endl << "updated, R is: " << endl << SO3_updated.matrix() << endl;Eigen::Quaterniond q_w(1.0,0.005,0.01,0.015);q_w.normalize();q_I = q_I * q_w;cout << endl << "updated, q is: " << q_I.coeffs().transpose()<< endl;cout << "updated, from q to R is: " <<endl << q_I.toRotationMatrix()<< endl;return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)project(test)
include_directories( "/usr/include/eigen3" )# 为使用 sophus ，需要使用 find_package 命令找到它
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )add_executable( test test.cpp )
target_link_libraries( test ${Sophus_LIBRARIES} )