没有合适的资源?快使用搜索试试~ 我知道了~
首页VINS论文推倒及代码解析
资源详情
资源评论
资源推荐
1
VINS 论文推导及代码解析
崔华坤 2018.9.3
一、总体框架 ................................................................................................................................... 3
1.1 图像和 IMU 预处理 .......................................................................................................... 3
1.2 初始化 ................................................................................................................................ 3
1.3 后端滑窗优化 .................................................................................................................... 3
1.4 闭环检测和优化 ................................................................................................................ 4
二、IMU 预积分 .............................................................................................................................. 4
2.1 当前时刻 PVQ 的连续形式 .............................................................................................. 4
2.2 当前时刻 PVQ 的中值法离散形式 .................................................................................. 5
2.3 两帧之间 PVQ 增量的连续形式 ...................................................................................... 5
2.4 两帧之间 PVQ 增量的欧拉法离散形式 .......................................................................... 6
2.5 两帧之间 PVQ 增量的中值法离散形式 .......................................................................... 6
2.6 连续形式下 PVQ 增量的误差、协方差及 Jacobian ....................................................... 7
2.7 离散形式的 PVQ 增量误差分析 ...................................................................................... 7
2.8 离散形式的 PVQ 增量误差的 Jacobian 和协方差 .......................................................... 8
三、后端非线性优化 ....................................................................................................................... 9
3.1 状态向量 ............................................................................................................................ 9
3.2 目标函数 ............................................................................................................................ 9
3.3 IMU 约束 .......................................................................................................................... 10
3.4 视觉约束 .......................................................................................................................... 12
四、前端视觉处理 ......................................................................................................................... 13
4.1 特征点检测 ...................................................................................................................... 13
4.2 特征点跟踪 ...................................................................................................................... 13
五、初始化..................................................................................................................................... 14
5.1 relativePose ....................................................................................................................... 15
2
5.2 GlobalSFM.construct ........................................................................................................ 15
5.3 solvePnP ............................................................................................................................ 17
5.4 visualInitialAlign .............................................................................................................. 17
六、边缘化 Marginalization 和 FEJ .............................................................................................. 20
6.1 边缘化和 Schur 补公式 .................................................................................................. 20
6.3 一个边缘化的例子 .......................................................................................................... 21
6.3 VINS 两种边缘化策略 .................................................................................................... 23
6.4 First Estimate Jacobian(FEJ) ............................................................................................. 27
七、闭环检测和优化 ..................................................................................................................... 28
7.1 闭环检测 .......................................................................................................................... 29
7.2 重定位 .............................................................................................................................. 30
7.3 闭环关键帧数据库 .......................................................................................................... 30
7.4 闭环优化 .......................................................................................................................... 31
7.5 实际程序逻辑 .................................................................................................................. 31
八、其他......................................................................................................................................... 33
8.1 选 KF 策略....................................................................................................................... 33
8.2 后端优化后的变量更新 .................................................................................................. 33
8.3 丢失后多地图融合 .......................................................................................................... 33
九、参考文献 ................................................................................................................................. 33
十、附录......................................................................................................................................... 34
10.1IMU 状态积分公式推导 ................................................................................................. 34
10.2 连续形式 IMU 误差动力学方程推导 .......................................................................... 35
10.3 IMU 离散误差动力学方程推导 .................................................................................... 36
10.4 IMU 角度误差对 k 时刻角度误差的 Jacobian 推导 ................................................... 38
10.5 IMU 角度误差对 k 时刻 bias 的 Jacobian 推导 ............................................................ 39
10.6 IMU 角度误差对 k+1 时刻的角度误差 Jacobian 推导 ................................................ 39
10.7 视觉误差项推导 ............................................................................................................ 39
10.8 视觉误差项的 Jacobian 推导 ........................................................................................ 40
3
一、总体框架
图 1 VINS 框架
VINS
[1]
的功能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测
及闭环优化。代码中主要开启了四个线程,分别是:前端图像跟踪、后端非线性优化(其中
初始化和 IMU 预积分在这个线程中)、闭环检测、闭环优化。
各个功能模块的作用主要有:
1.1 图像和 IMU 预处理
图像:提取图像 Harris 角点,利用金字塔光流跟踪相邻帧,通过 RANSAC 去除异常点,
最后将跟踪到的特征点 push 到图像队列中,并通知后端进行处理。
IMU:将 IMU 数据进行积分,得到当前时刻的位置、速度和旋转(PVQ),同时计算在
后端优化中将用到的相邻帧的预积分增量,及预积分误差的 Jacobian 矩阵和协方差项。
1.2 初始化
首先,利用 SFM 进行纯视觉估计滑窗内所有帧的位姿及 3D 点逆深度,最后与 IMU 预
积分进行对齐求解初始化参数。
1.3 后端滑窗优化
将视觉约束、IMU 约束和闭环约束放在一个大的目标函数中进行非线性优化,求解滑窗
内所有帧的 PVQ、bias 等。
4
图 2 滑窗优化示意图
1.4 闭环检测和优化
利用 DBoW 进行闭环检测,当检测成功后进行重定位,最后对整个相机轨迹进行闭环优
化。
二、IMU 预积分
图 3 IMU 预积分示意图
2.1 当前时刻 PVQ 的连续形式
将第 k 帧和第 k+1 帧之间的所有 IMU 进行积分,可得第 k+1 帧的位置、速度和旋转
(PVQ),作为视觉估计的初始值,这里的旋转采用的四元数。
(1)
其中,
和
为 IMU 测量的加速度和角速度,是在 Body 自身坐标系,world 坐标系是
IMU 所在的惯导系,上式的旋转公式推导可参考附录 10.1。
k
k+1
5
2.2 当前时刻 PVQ 的中值法离散形式
公式(1)给出的是连续时刻的相机当前 PVR 的迭代公式,为了跟代码一致,下面给出基
于中值法的公式,这与 Estimator::processIMU()函数中的 Ps[j]、Rs[j]和 Vs[j]是一致的,IMU
积分出来的第 j 时刻的物理量可以作为第 j 帧图像的初始值。
(2)
其中:
(3)
2.3 两帧之间 PVQ 增量的连续形式
通过观察公式(1)可知,IMU 的预积分需要依赖与第 k 帧的 v 和 R,当我们在后端进行
非线性优化时,需要迭代更新第 k 帧的 v 和 R,这将导致我们需要根据每次迭代后值重新进
行积分,这将非常耗时。因此,我们考虑将优化变量从第 k 帧到第 k+1 帧的 IMU 预积分项
中分离开来,通过对公式(1)左右两侧各乘
,可化简为:
(4)
其中:
(5)
这样我们就得到了连续时刻的 IMU 预积分公式,可以发现,上式得到的 IMU 预积分的
值只与不同时刻的
和
相关。
这里我们需要重新讨论下公式(5)的预积分公式,以
为例,我们发现它是与 IMU 的
剩余40页未读,继续阅读
TracyMc
- 粉丝: 1
- 资源: 4
上传资源 快速赚钱
- 我的内容管理 收起
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
会员权益专享
最新资源
- RTL8188FU-Linux-v5.7.4.2-36687.20200602.tar(20765).gz
- c++校园超市商品信息管理系统课程设计说明书(含源代码) (2).pdf
- 建筑供配电系统相关课件.pptx
- 企业管理规章制度及管理模式.doc
- vb打开摄像头.doc
- 云计算-可信计算中认证协议改进方案.pdf
- [详细完整版]单片机编程4.ppt
- c语言常用算法.pdf
- c++经典程序代码大全.pdf
- 单片机数字时钟资料.doc
- 11项目管理前沿1.0.pptx
- 基于ssm的“魅力”繁峙宣传网站的设计与实现论文.doc
- 智慧交通综合解决方案.pptx
- 建筑防潮设计-PowerPointPresentati.pptx
- SPC统计过程控制程序.pptx
- SPC统计方法基础知识.pptx
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论3