笔记"/>
手写VIO笔记
原文个人博客链接
最近深蓝学院在视觉slam之后又推出了进阶版的手写VIO教程,看上去能学到很多东西的样子,唯一一点不足之处在于价格实在是太贵了好吧!!!表示穷学生党学不起,好在最近找了几个志同道合的朋友一起承担这个费用才勉强能获得学习资源。怀着强烈的期待与好奇并希望借这个机会提高一下自己的能力,我决定每一次的课程都写一篇笔记来巩固自己学的东西,并把课后习题贴在这里,逼迫自己好好学习,day day up!
在预备知识中,主要讲诉了两个问题。视觉与IMU融合的方法与特点概述以及三维刚体的表达中使用的数学知识,主要就是四元数与李群。
VIO概述
IMU
反馈频率较高>100Hz,一般都大于500Hz吧,但是会受通信质量影响
受温度、振动的影响较大。会有稳漂现象,具体可以参考飞控代码对陀螺仪的解算与补偿办法
相机
以图像的形式记录数据,帧数较低(15~60Hz)
通过特征点推断相机运动
两者的对比
方案 | IMU | 视觉 |
---|---|---|
优势 | 快速响应 | 不产生漂移 |
不受成像质量影响 | 直接测量旋转漂移 | |
角速度比较准确 | ||
可估计绝对角度 | ||
劣势 | 存在零偏 | 受图像遮挡 |
低精度IMU积分位姿发散 | 单目无法测量尺度 | |
高精度价格昂贵 | 纯旋转运动时无法估计 | |
快速运动时容易丢失 |
松融合:各自计算出位姿然后通过卡尔曼等方法融合
紧融合:直接利用IMU数据和图像特征数据一起估计位姿
三维空间中的刚体
其中旋转矩阵和四元数比较简单,就不仔细写了,主要要理解利用李群的求导过程。
旋转矩阵
四元数
SE(3)、SO(3)
左扰动雅可比
∂ ( R p ) ∂ φ = lim φ → 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 ( I + φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 φ ∧ R p φ = lim φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ \begin{aligned}\frac{\partial(\mathbf{R} \mathbf{p})}{\partial \varphi}&=\lim _{\varphi \rightarrow \mathbf{0}} \frac{\exp \left(\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) \mathbf{p}-\exp \left(\phi^{\wedge}\right) \mathbf{p}}{\varphi} \\\\&=\lim _{\varphi \rightarrow 0} \frac{\left(\mathbf{I}+\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) \mathbf{p}-\exp \left(\phi^{\wedge}\right) \mathbf{p}}{\varphi} \\\\&=\lim _{\varphi \rightarrow 0} \frac{\varphi^{\wedge} \mathbf{R} \mathbf{p}}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-(\mathbf{R} \mathbf{p})^{\wedge} \varphi}{\varphi}=-(\mathbf{R} \mathbf{p})^{\wedge} \end{aligned} ∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧
右扰动雅可比
∂ ( R p ) ∂ φ = lim φ → 0 R exp ( φ ∧ ) p − R p φ = lim φ → 0 R ( I + φ ∧ ) p − R p φ = lim φ → 0 R φ ∧ p φ = lim φ → 0 − R p ∧ φ φ = − R p ∧ \begin{aligned}\frac{\partial(\mathbf{R} \mathbf{p})}{\partial \varphi}&=\lim _{\varphi \rightarrow 0} \frac{\mathbf{R} \exp \left(\varphi^{\wedge}\right) \mathbf{p}-\mathbf{R} \mathbf{p}}{\varphi} \\\\&=\lim _{\varphi \rightarrow 0} \frac{\mathbf{R}\left(\mathbf{I}+\varphi^{\wedge}\right) \mathbf{p}-\mathbf{R} \mathbf{p}}{\varphi} \\\\&=\lim _{\varphi \rightarrow 0} \frac{\mathbf{R} \varphi^{\wedge} \mathbf{p}}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-\mathbf{R} \mathbf{p}^{\wedge} \varphi}{\varphi}=-\mathbf{R} \mathbf{p}^{\wedge} \end{aligned} ∂φ∂(Rp)=φ→0limφRexp(φ∧)p−Rp=φ→0limφR(I+φ∧)p−Rp=φ→0limφRφ∧p=φ→0limφ−Rp∧φ=−Rp∧
代码实现证明,对于很小的w四元数跟新和右扰动跟新差别很小,可近似认为相等
gitbub代码连接
更多推荐
手写VIO笔记
发布评论