深蓝学院-手写VIO作业-第一章
文章目录
- 一、VIO文献阅读
- 二、四元数和李代数更新
- 代码编写
- 最终运行结果截图:
- 三、其他导数
- 1.
- 2.
一、VIO文献阅读
阅读VIO 相关综述文献[a],回答以下问题:
视觉与IMU进行融合之后有何优势?
- 视觉传感器在大多数纹理比较丰富的场景中效果较好,但是如果遇到白墙,或者纯色的一些特殊场景,基本上无法提取特征,也就无法工作;并且频率不能太高,15-60Hz居多。
- IMU存在漂移现象,随着时间的推移,误差累积越来越多,但在短时间内,IMU的精度很高,典型的6轴IMU频率大于或等于100Hz。
我的理解是,因为视觉和IMU都有各自相应的优势和缺点,整合两个传感器的优势,取长补短。因此,融合之后的优势是:
视觉与IMU的融合可以借助IMU较高的采样频率,进而提高系统的输出频率。
视觉与IMU的融合可以提高视觉的鲁棒性,如视觉SLAM因为某些运动或场景出现的错误结果。
视觉与IMU的融合可以有效的消除IMU的积分漂移。
视觉与IMU的融合能够校正IMU的Bias。
单目与IMU的融合可以有效解决单目尺度不可观测的问题。
可以应对快速的运动变化,相机在快速运动过程中会出现运动模糊。
有哪些常见的视觉+IMU 融合方案?有没有工业界应用的例子?
常见的融合方案:
2020年推出的ORB-SLAM3方案,提供了双目+IMU的融合方案,并且ORB-SLAM3在深蓝学院公开课也有介绍。
工业界应用
VIO主要是应用自动驾驶和AR、VR领域;比如可以使用手机进行AR换鞋(京东淘宝上有些店铺是支持AR试鞋的)。以硬件为主打的XR头盔设备,目前在工业领域都一些落地的场景,如西门子的AR头盔的工业展示,飞机驾驶员的培训。
在学术界,VIO 研究有哪些新进展?有没有将学习方法用到VIO中的例子?
目前VIO和其他传感器的融合也是一个比较受欢迎的研究热点,比如VIO和激光雷达的融合,和GPS的融合。
- 2019, RAL, Visual-Inertial Localization with Prior LiDAR Map Constraints --视觉 VIO 和激光地图。
- 2018, arxiv, A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors --VINS-Mono 的扩展版,能 融合 GPS、单目、双目等信息。
- 2017, ICRA, Vins on wheels --系统分析了 VIO + 轮速计系统的可观性。
- 2019, ICRA, Visual-Odometric Localization and Mapping for Ground Vehicles Using SE(2)-XYZ Constraints --SE2-XYZ 的模型来对地 面轮速机器人进行参数化。
VIO+深度学习
- VINet : Visual-inertial odometry as a sequence-to-sequence learning problem.就整体而言,VINet是首次使用DL的框架来解决VIO问题,目前所披露的实验表现出了一定的实用价值。
- Unsupervised Deep Visual-Inertial Odometry with Online Error Correction for RGB-D Imagery.此方法可以在没有Camera-IMU外参的情况下基于学习进行VIO网络学习整合IMU测量并生成估计轨迹,然后根据相对于像素坐标的空间网格的缩放图像投影误差的雅可比行列式在线校正。
参考博客:视觉惯性里程计 综述
题目:A review of visual inertial odometry from filtering and optimisation perspectives
期刊:Advanced Robotics | Volume 29,2015 - Issue 20
作者及研究机构信息:
翻译:VIO综述论文
二、四元数和李代数更新
课件提到了可以使用四元数或旋转矩阵存储旋转变量。当我们用计算出来的ω\omegaω对某旋转更新时,有两种不同方式:
R←Rexp(ω∧)或q←q⊗[1,12ω]TR \leftarrow R \exp(\omega^{\wedge})\\ 或 q\leftarrow q \otimes [1, \frac{1}{2}\omega]^{T} R←Rexp(ω∧)或q←q⊗[1,21ω]T
请编程验证对于小量ω\omegaω = [0.01, 0.02, 0.03]T,两种方法得到的结果非常接近,实践当中可视为等同。因此,在后文提到旋转时,我们并不刻意区分旋转本身是q 还是R,也不区分其更新方式为上式的哪一种。
代码编写
按照公式进行编程,参照了课本p48和p87页代码中对于eigen库和sophus库的使用。
旋转向量参照课本上的例子,将其设置为沿z轴旋转45度。利用旋转矩阵构造对应的四元数,利用 Sophus::SO3d::hat可以求得旋转向量的反对称矩阵。
具体代码如下:
int main()
{//旋转矩阵Matrix3d rotation_matrix = Matrix3d::Identity();AngleAxisd rotation_vector(M_PI/4, Vector3d(0,0,1)); //表示沿Z轴旋转45度rotation_matrix = rotation_vector.toRotationMatrix();cout<<"R: "<<endl<<rotation_matrix<<endl<<endl;//构造对应的四元数Eigen::Quaterniond q(rotation_matrix);cout << "q :"<< endl << q.coeffs().transpose() <<endl<<endl;//使用对数映射获得它的李代数Sophus::SO3d SO3_R(rotation_matrix);cout << "so3 :"<< endl << SO3_R.log().transpose() <<endl<<endl;//更新量omegaEigen::Vector3d omega(0.01, 0.02, 0.03); //更新量// 李代数更新Matrix3d skew_symmetric_matrix = Sophus::SO3d::hat(omega).matrix();cout<<"SO3 hat= "<<endl<< skew_symmetric_matrix<<endl<<endl;Sophus::SO3d SO3_updated = SO3_R * Sophus::SO3d::exp(omega);cout<<"SO3 updated = "<<endl<<(SO3_R * Sophus::SO3d::exp(omega)).matrix()<<endl<<endl;// 四元数更新Eigen::Quaterniond q_update(1, omega(0)/2, omega(1)/2, omega(2)/2);Eigen::Quaterniond q_updated = q * q_update.normalized();cout<<"Quaterniond updated = "<< endl << q_updated.toRotationMatrix() <<endl<<endl;//衡量两种方法之间的差距cout<<"disparity = "<< endl <<SO3_updated.matrix().transpose()*q_updated.toRotationMatrix()<<endl;
}
最终运行结果截图:
从实验结果可以看出,两种不同方式得到的ω\omegaω对某旋转更新结果差别很小,(这里需要注意的一点是,因为旋转矩阵是在李群上的,李群是没有加减法的,所以可以使用第一个R的逆或者转置矩阵乘第二个R矩阵,越接近单位矩阵,则说明两个矩阵之间的差距越小)差距在10−610^{-6}10−6以内,因此并不刻意区分旋转本身是q 还是R。
三、其他导数
使用右乘so(3)\mathfrak{so}(3)so(3),推导以下导数:
1.
d(R−1p)dR\frac{d(\mathbf{R}^{-1}p)}{d\mathbf{R}} dRd(R−1p)
右扰动:
d(R−1p)dR=d(R−1p)dφ=limφ→0(Rexp(φ∧))−1p−R−1pφ=limφ→0(exp(φ∧))−1R−1p−R−1pφ=limφ→0exp(−φ∧)R−1p−R−1pφ泰勒展开≈limφ→0(I−φ∧)R−1p−R−1pφ=limφ→0−φ∧R−1pφ根据slam课程中表1中提供的性质u∧v=−v∧u可得:=limφ→0(R−1p)∧φφ=(R−1p)∧\begin{aligned} \frac{d(R^{-1} p)}{d R} &= \frac{d(R^{-1} p)}{d \varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{(R \exp(\varphi^{\wedge}))^{-1}p-R^{-1}p}{\varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{(\exp(\varphi^{\wedge}))^{-1}R^{-1}p-R^{-1}p}{\varphi} \\ &= \lim\limits_{\varphi \to 0} \frac{\exp(- \varphi^{\wedge})R^{-1}p-R^{-1}p}{\varphi} \\ 泰勒展开&\approx \lim\limits_{\varphi \to 0} \frac{(I- \varphi^{\wedge})R^{-1}p-R^{-1}p}{\varphi} \\&= \lim\limits_{\varphi \to 0} \frac{- \varphi^{\wedge}R^{-1}p}{\varphi} \\ 根据slam课程中表1中提供的性质u^{\wedge}v = -v^{\wedge}u可得: &= \lim\limits_{\varphi \to 0} \frac{(R^{-1}p)^{\wedge}\varphi}{\varphi} \\ &=(R^{-1}p)^{\wedge} \end{aligned} dRd(R−1p)泰勒展开根据slam课程中表1中提供的性质u∧v=−v∧u可得:=dφd(R−1p)=φ→0limφ(Rexp(φ∧))−1p−R−1p=φ→0limφ(exp(φ∧))−1R−1p−R−1p=φ→0limφexp(−φ∧)R−1p−R−1p≈φ→0limφ(I−φ∧)R−1p−R−1p=φ→0limφ−φ∧R−1p=φ→0limφ(R−1p)∧φ=(R−1p)∧
2.
dln(R1R2−1)∨dR2\frac{d ln(\mathbf{R}_1\mathbf{R}_2^{-1})^{\vee}}{d\mathbf{R_2}} dR2dln(R1R2−1)∨
这里需要用BCH线性近似表达:
ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量时Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量时\ln(\exp(\phi_1^{\wedge})\exp(\phi_2^{\wedge}))^{\vee} \approx \begin{cases} \mathcal{J}_l(\phi_2)^{-1}\phi_1 + \phi_2 &当\phi_1为小量时\\ \mathcal{J}_r(\phi_1)^{-1}\phi_2 + \phi_1 &当\phi_2为小量时 \end{cases} ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1当ϕ1为小量时当ϕ2为小量时
还用到了如下性质:
(Cu)∧≡Cu∧CTexp((Cu)∧)≡Cexp(u∧)CT旋转矩阵R−1=RT(\mathbf{C}u)^{\wedge}\equiv \mathbf{C}u^{\wedge}\mathbf{C}^T \\ \exp((\mathbf{C}u)^{\wedge}) \equiv \mathbf{C}\exp(u^{\wedge})\mathbf{C}^T \\ 旋转矩阵 R^{-1}=R^T (Cu)∧≡Cu∧CTexp((Cu)∧)≡Cexp(u∧)CT旋转矩阵R−1=RT
通过右扰动的方式计算上边的公式:
dln(R1R2−1)∨dR2=limφ2→0ln(R1(R2exp(φ2∧))−1)∨−ln(R1R2−1)∨φ2=limφ2→0ln(R1exp(−φ2∧)R2−1)∨−ln(R1R2−1)∨φ2=limφ2→0ln(R1R2−1R2exp(−φ2∧)R2T)∨−ln(R1R2−1)∨φ2=limφ2→0ln(R1R2−1exp((−R2φ2)∧))∨−ln(R1R2−1)∨φ2BCH近似≈Jr(ln(R1R2−1)∨)−1(−R2φ2)+ln(R1R2−1)∨−ln(R1R2−1)∨φ2=−Jr(ln(R1R2−1)∨)−1⋅R2\begin{aligned} \frac{d \ln(R_1R_2^{-1})^{\vee}}{d R_2} &= \lim\limits_{\varphi_2 \to 0} \frac{\ln(R_1 (R_2 \exp(\varphi_2^{\wedge}))^{-1})^{\vee}-\ln(R_1 R_2^{-1})^{\vee}}{\varphi_2} \\&= \lim\limits_{\varphi_2 \to 0} \frac{\ln(R_1 \exp(-\varphi_2^{\wedge})R_2^{-1})^{\vee}-\ln(R_1 R_2^{-1})^{\vee}}{\varphi_2} \\&= \lim\limits_{\varphi_2 \to 0} \frac{\ln(R_1 R_2^{-1} R_2 \exp(-\varphi_2^{\wedge})R_2^T)^{\vee}-\ln(R_1 R_2^{-1})^{\vee}}{\varphi_2} \\&= \lim\limits_{\varphi_2 \to 0} \frac{\ln(R_1 R_2^{-1} \exp((-R_2\varphi_2)^{\wedge}))^{\vee}-\ln(R_1 R_2^{-1})^{\vee}}{\varphi_2} \\ BCH近似 &\approx \frac{\mathcal{J_r}(\ln(R_1R_2^{-1})^{\vee})^{-1}(-R_2\varphi_2) + \ln(R_1R_2^{-1})^{\vee}- \ln(R_1R_2^{-1})^{\vee}}{\varphi_2} \\ &= -\mathcal{J_r}(\ln(R_1R_2^{-1})^{\vee})^{-1} \cdot R_2 \end{aligned} dR2dln(R1R2−1)∨BCH近似=φ2→0limφ2ln(R1(R2exp(φ2∧))−1)∨−ln(R1R2−1)∨=φ2→0limφ2ln(R1exp(−φ2∧)R2−1)∨−ln(R1R2−1)∨=φ2→0limφ2ln(R1R2−1R2exp(−φ2∧)R2T)∨−ln(R1R2−1)∨=φ2→0limφ2ln(R1R2−1exp((−R2φ2)∧))∨−ln(R1R2−1)∨≈φ2Jr(ln(R1R2−1)∨)−1(−R2φ2)+ln(R1R2−1)∨−ln(R1R2−1)∨=−Jr(ln(R1R2−1)∨)−1⋅R2
代码地址:https://gitee.com/ximing689/vio-learning.git
深蓝学院-手写VIO作业-第一章相关推荐
- 深蓝学院-手写VIO作业-第二章
文章目录 一.基础作业,必做 环境配置说明 a. ROS环境搭建 b. Matlab安装 1.设置IMU 仿真代码中的不同的参数,生成Allen 方差标定曲线. a. 对于非ROS:生成运动imu数据 ...
- 从零开始手写VIO第三章作业(含关键点细节及思维过程)
文章目录 前言·与同主题博文的不同 1.代码修改 1.1阻尼因子 µ 随着迭代变化的曲线图 1.2完成曲线y = ax^2^ + bx + c的参数估计 1.3实现其他更优秀的阻尼因子策略 2.公式推 ...
- 《视觉SLAM进阶:从零开始手写VIO》第一讲作业
目录 1 视觉与IMU融合之后有何优势? 2 有哪些常见的视觉+IMU融合方案?有没有工业界应用的例子? 3 在学术界,VIO研究有哪些新进展?有没有将学习方法应用到VIO的例子? 4 四元数和李代数 ...
- 手写VIO --学习笔记 - Part1
目录 一.VIO文献阅读 二.四元数和李代数更新 三.其他导数 参考 一.VIO文献阅读 阅读VIO 相关综述文献,回答以下问题: 1.视觉与IMU进行融合之后有何优势? IMU:(Inertial ...
- 深蓝学院《从零开始手写VIO》作业七
深蓝学院<从零开始手写VIO>作业七 深蓝学院<从零开始手写VIO>作业七 深蓝学院<从零开始手写VIO>作业七 将第二讲中的仿真数据(视觉特征,imu数据)接入V ...
- 深蓝学院《从零开始手写VIO》作业五
深蓝学院<从零开始手写VIO>作业五 1. 完成Bundle Adjustment求解器 2. 完成测试函数 3. 论文总结 1. 完成Bundle Adjustment求解器 完成单目 ...
- 深蓝学院《从零开始手写VIO》作业一
深蓝学院<从零开始手写VIO>作业一 深蓝学院<从零开始手写VIO>作业一 1. VIO文献阅读 1.1 视觉与IMU进行融合之后有何优势? 1.2 有哪些常见的视觉+IMU融 ...
- 深蓝学院《从零开始手写VIO》作业六
深蓝学院<从零开始手写VIO>作业五 深蓝学院<从零开始手写VIO>作业六 1. 证明题 2. 代码题 深蓝学院<从零开始手写VIO>作业六 1. 证明题 证明Dy ...
- 深蓝学院《从零开始手写VIO》作业四
深蓝学院<从零开始手写VIO>作业四 深蓝学院<从零开始手写VIO>作业四 1. 信息矩阵分析 2. Bundle Adjustment信息矩阵的计算 深蓝学院<从零开始 ...
最新文章
- python学习三:列表,元组
- 数据结构源码笔记(C语言):线性表的顺序表示
- python数据库操作之pymysql模块和sqlalchemy模块(项目必备)
- WeChat微信小程序网易云音乐播放链接方法
- 调用百度 Echarts 显示重庆市地图
- 【jquery模仿net控件】初步GridView模型实现,及其简单应用
- python tkinter选择路径控件_如何使用tkinter在Python中选择目录并存储位置
- 外部类、内部类、局部内部类、匿名类(Java)
- 基于深度极限学习机DELM的回归预测
- java实践体会,java实践心得体会范文3篇
- 基于java题库及试卷管理模块的设计与开发(含源文件)
- 微信资源混淆AndResGuard原理
- Python词频统计的3种方法
- AH8063,输入6.6V-30V,4A大电流,三节锂电池充电管理芯片iC
- 搭建单节点ELK日志收集
- “被裁员之前,没得到任何风声,措手不及...” 一个在职6年测试工程师内心独白
- 2019加密市场金融借贷领域研究报告(英文版) | TokenInsight
- 基于Python实现的全球新冠病毒数据分析
- Hi3516的venc编码模块处理流程
- linux运维cadn,Aprende an elaborar un amasamiento tГЎntrico citaciГіn
热门文章
- 2020-12-31小黄自学文档整理(其实是小白,因为姓黄)
- 【K8S 基本理论及架构原理】
- win10的升级方法
- Ubuntu下求生之路2Linux服务器搭建(官方战役,三方药抗)
- Word2007 如何自动生成目录的方法
- 三国志战略版:Daniel_蔡文姬_武锋阵_1.13日更新分析
- Python 小型项目大全 21~25
- 为了解决jetson tx2的内存不足。挂载sd卡,并且使用docker在sd中安装jetPack的镜像。
- term长度超限制问题
- 输入任意一种物质,要求输出其每种元素的数量。比如输入 CaCO3,其组成分别为 Ca:1,C:1,O:3,输出 Ca1C1O3