手写VIO笔记--概述

原文个人博客链接
最近深蓝学院在视觉slam之后又推出了进阶版的手写VIO教程，看上去能学到很多东西的样子，唯一一点不足之处在于价格实在是太贵了好吧！！！表示穷学生党学不起，好在最近找了几个志同道合的朋友一起承担这个费用才勉强能获得学习资源。怀着强烈的期待与好奇并希望借这个机会提高一下自己的能力，我决定每一次的课程都写一篇笔记来巩固自己学的东西，并把课后习题贴在这里，逼迫自己好好学习，day day up！

在预备知识中，主要讲诉了两个问题。视觉与IMU融合的方法与特点概述以及三维刚体的表达中使用的数学知识，主要就是四元数与李群。

VIO概述

IMU

反馈频率较高>100Hz，一般都大于500Hz吧，但是会受通信质量影响
受温度、振动的影响较大。会有稳漂现象，具体可以参考飞控代码对陀螺仪的解算与补偿办法

相机

以图像的形式记录数据，帧数较低（15～60Hz）
通过特征点推断相机运动

两者的对比

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

松融合：各自计算出位姿然后通过卡尔曼等方法融合
紧融合：直接利用IMU数据和图像特征数据一起估计位姿

三维空间中的刚体

其中旋转矩阵和四元数比较简单，就不仔细写了，主要要理解利用李群的求导过程。
旋转矩阵
四元数
SE（3）、SO（3）

左扰动雅可比

$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp \left({\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{\left(\mathbf{I}+{\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\mathbf{R}\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\phi }{\phi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$$

右扰动雅可比

$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\exp \left({\phi }^{\wedge }\right)\mathbf{p}-\mathbf{R}\mathbf{p}}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\left(\mathbf{I}+{\phi }^{\wedge }\right)\mathbf{p}-\mathbf{R}\mathbf{p}}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}{\phi }^{\wedge }\mathbf{p}}{\phi }=\underset{\phi \to 0}{\lim }\frac{-\mathbf{R}{\mathbf{p}}^{\wedge }\phi }{\phi }=-\mathbf{R}{\mathbf{p}}^{\wedge }\end{array}$$

代码实现证明，对于很小的w四元数跟新和右扰动跟新差别很小，可近似认为相等
gitbub代码连接