LR Robust Stereo VIO for Fast Autonomous Flight
Abstract
我们展示说我们的Stereo MSCKF在算力上跟state-of-the-art的单目方案是可比的, 而且提供了很大的鲁棒性.
1. Introduction
贡献
- 第一个开源的filter-based 立体相机VIO, 可以在板跑是
- 提供详实的实验数据, 跟OKVIS, ROVIO, VINS-MONO比对, 在精度, 效率和鲁棒性上.
- 有一个快速飞行的数据集
2. Related Work
现存的紧耦合VIO方案, 可以分为优化的([4, 15, 16]) 和 滤波的([1, 5, 20]).
滤波的方案, 一般就用EKF[1]. 或者是Uncented KF[19], 会更高效. [23, 2, 24] 也提出了FEJ和 Observability Constraint 来提升VIO在滤波狂阿基的鲁棒性, 也提升了估计精度.
最近的工作里, [5, 25]用直接法的工作更进一步提升了精度和鲁棒性.
只有很少部分的是为了多相机, 或者是立体相机设计的[4, 16, 26, 27].
[26]中, stereo VI 跑到了6.25hz, 更像是一个概念的验证. [4] 提出了一个更完成的优化框架. [16] 引入了直接法到stereo VIO来提升精度. 这三个方案都是基于优化的.
[27] 提出了一个基于滤波的stereo VIO, 基于平方根inverse filter [18].
3. Filter Description
IMU的状态:
^I_G\mathbf{q}^{\top} & \mathbf{b}_{g}^{\top} & G_{\mathbf{v}_{I}}^{\top} & \mathbf{b}_{a}^{\top} & { }^{G} \mathbf{p}_{I}^{\top} & { }_{C}^{I} \mathbf{q}^{\top} & \left.{ }^{I} \mathbf{p}_{C}^{\top}\right)
\end{array}\right.
\]
这里四元数 \(^I_G q\) 表示从惯性系到body系. body系就是IMU系.
向量 \(^Gv_I \in \mathcal{R}^3\) 和 \(^Gp_I \in \mathcal{R}^3\) 表示body系在惯性系的速度和位置.
四元数 \(^I_Cq\) 和 \(^Ip_C\) 表示相机和body系的相对旋转和平移.
使用 true IMU state会导致奇异性?, 在结果的协方差矩阵, 因为额外的状态向量里协方差矩阵的额外的约束.
error IMU state被定义为:
用来使用位置, 速度和偏置的standard additive error (e.g. $ ^G \tilde{p}_I = ^G p_I - ^G \hat{p}_I$ ) 对于四元数, 误差四元数 \(\delta \mathbf{q}=\mathbf{q} \otimes \hat{\mathbf{q}}^{-1}\)
\]
这里 \(^G_I \tilde{\theta} \in R^3\) 表示小旋转.
最终, N 个相机状态如下:
\tilde{\mathbf{x}}_{I}^{\top} & \tilde{\mathbf{x}}_{C_{1}}^{\top} & \cdots & \left.\tilde{\mathbf{x}}_{C_{N}}^{\top}\right)^{\top}
\end{array}\right.
\]
每个相机的error state的定义是:
C_{i} & \tilde{\boldsymbol{\theta}}^{\top}
\end{array}^{G} \tilde{\mathbf{p}}_{C_{i}}^{\top}\right)^{\top}
\]
为了保持计算的复杂度, 雨鞋相机状态需要被边缘化, 当相机的状态量到一个阈值到时候.
A. Process Model
估计的IMU状态的continues dynamics:
{ }_{G} \dot{\mathbf{q}}=\frac{1}{2} \Omega(\hat{\boldsymbol{\omega}})_{G}^{I} \hat{\mathbf{q}}, \quad \dot{\hat{\mathbf{b}}}_{g}=\mathbf{0}_{3 \times 1} \\
{ }^{G} \dot{\hat{\mathbf{v}}}=C\left(\begin{array}{c}
^I_G\hat{q}
\end{array}\right)^{\top} \hat{\mathbf{a}}+{ }^{G} \mathbf{g} \\
\dot{\hat{b}}_{a}=\mathbf{0}_{3 \times 1}, \quad{ }^{G} \dot{\hat{\mathbf{p}}}_{I}={ }^{G} \hat{\mathbf{v}} \\
{ }_{C} \dot{\hat{\mathbf{q}}}=\mathbf{0}_{3 \times 1}, \quad{ }^{I} \dot{\hat{\mathbf{p}}}_{C}=\mathbf{0}_{3 \times 1}
\end{array} \tag1
\]
这里 \(\hat{\omega} \in R^3\) 和 \(\hat{a} \in R^3\) 是IMU除去偏置的测量
\]
同时:
-\left[\hat{\omega}_{\times}\right] & \omega \\
-\omega^{\top} & 0
\end{array}\right)
\]
linearized continuous dynamics 对于 error IMU state:
\]
这里 \(\mathbf{n}_{I}^{\top}=\left(\mathbf{n}_{g}^{\top} \mathbf{n}_{w g}^{\top} \mathbf{n}_{a}^{\top} \mathbf{n}_{w a}^{\top}\right)^{\top}\) .
为了解决离散时间观测, 我们用了一个4th Runga-Kutta数值integration of Eq(1) 来传播估计的IMU状态. 为了传播状态的不确定性, Eq(2)的离散时间状态转换矩阵和离散时间噪声协方差需要先计算.
\boldsymbol{\Phi}_{k} &=\boldsymbol{\Phi}\left(t_{k+1}, t_{k}\right)=\exp \left(\int_{t_{k}}^{t_{k+1}} \mathbf{F}(\tau) d \tau\right) \\
\mathbf{Q}_{k} &=\int_{t_{k}}^{t_{k+1}} \boldsymbol{\Phi}\left(t_{k+1}, \tau\right) \mathbf{G Q G} \boldsymbol{\Phi}\left(t_{k+1}, \tau\right)^{\top} d \tau
\end{aligned}
\]
这里 \(\mathbf{Q}=\mathbb{E}\left[\mathbf{n}_{I} \mathbf{n}_{I}^{\top}\right]\) 是持续时间噪声协方差. 这样IMU状态的传播协方差是:
\]
整个状态的协方差是:
\mathbf{P}_{I I_{k \mid k}} & \mathbf{P}_{I C_{k \mid k}} \\
\mathbf{P}_{I C_{k \mid k}}^{\top} & \mathbf{P}_{C C_{k \mid k}}
\end{array}\right)
\]
整个传播的不确定性是:
\mathbf{P}_{I I_{k+1 \mid k}} & \mathbf{\Phi}_{k} \mathbf{P}_{I C_{k \mid k}} \\
\mathbf{P}_{I C_{k \mid k}}^{\top} \mathbf{\Phi}_{k}^{\top} & \mathbf{P}_{C C_{k \mid k}}
\end{array}\right)
\]
当新接受到图像的时候, 状态需要被增广(augmented). 新相机状态的pose可以从最近的IMU状态获得
\]
增广的协方差就是:
\mathbf{I}_{21+6 N} \\
\mathbf{J}
\end{array}\right) \mathbf{P}_{k \mid k}\left(\begin{array}{c}
\mathbf{I}_{21+6 N} \\
\mathbf{J}
\end{array}\right)^{\top}
\]
B. Measurement Model
立体观测\(z^j_i\)可以表示为:
\]
注意我们可以让\(z\) 是 \(R^3\), 但是要rectified. 如果是 \(R^4\) 的话, 就不需要同一个特征的观测要在一个平面上了.
这个测量的残差近似为:
\]
这里 \(n_i^j\) 是测量误差. 两个 \(H\) 是jacobian.
通过积累一个特征\(f_j\) 的多个观测, 我们有:
\]
如[1]中所述, 因为\(^Gp_j\) 是用相机位姿计算拿到, 它的不确定性也是和状态中的相机位姿相关的.
为了保证它的不确定性不影响残差, Eq 5是投影到零空间的.
\]
C. Observability Constraint
一个天真的EKF VIO操作会得到很多yaw的虚假信息. 这是因为process的线性化点, 和测量步骤是在不同的时刻.
有很多不同的办法维护滤波器的一致性, 包括FEJ[23], Observability Constrainted EKF [23], Robocentric Mapping Filter[29].
在我们的应用中, OC-EKF因为两个理由用了.
- 不想FEJ-EKF, OC-EKF不需要很依赖准确的初值.
- 跟Robocentric Mapping Filter比, camera poses in the state vector can be represented with respect to the inertial frame instead of the latest IMU frame so that the uncertainty of the existing camera
states in the state vector is not affected by the uncertainty of the latest IMU state during the propagation step.
D. Filter Update Mechanism
...
E. Image Processing Frontend
我们用FAST和KLT.
注意我们在立体特征匹配, 也用KLT.
经验上, 深度大于1m的角点可以可靠的用KLT匹配.
4. Experiments
在V2_03_difficlut上跑的不好, 因为我们用了KLT. 持续的光照不稳定导致了失败.
5. Conclusion
可以跑到17.5m/s快.
LR Robust Stereo VIO for Fast Autonomous Flight的更多相关文章
- 一步一步学习S-MSCKF(一)连续时间IMU误差状态运动模型
1 IMU真实状态运动模型 状态向量: \(x_{I}=\left[{{_{G}^{I}{q(t)}}^{T},{b_{g}(t)}^{T},{^{G}v_{I}(t)}^{T},{b_{a}(t)} ...
- 83 项开源视觉 SLAM 方案够你用了吗?
作者:吴艳敏 来源:83 项开源视觉 SLAM 方案够你用了吗? 前言 1. 本文由知乎作者小吴同学同步发布于https://zhuanlan.zhihu.com/p/115599978/并持续更新. ...
- MSCKF_VIO:MSCKF的双目版本
论文:MSCKF的双目版本 Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight 下载地址:点击 源码地址:https:/ ...
- ROSCon 2016视频和幻灯片发布 ROS机器人操作系统重要参考资料
ROSCon 2016视频和幻灯片发布 By Tully Foote on 十月19,2016 7:28 AM 全部PPT下载地址:http://pan.baidu.com/s/1gf2sn2F RO ...
- 计算机视觉code与软件
Research Code A rational methodology for lossy compression - REWIC is a software-based implementatio ...
- SVO+PL-SVO+PL-StVO
PL-SVO是基于点.线特征的半直接法单目视觉里程计,我们先来介绍一下基于点特征的SVO,因为是在这个基础上提出的. [1]References: SVO: Fast Semi-Direct ...
- APM程序分析-ArduCopter.cpp
该文件是APM的主文件. #define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter ...
- CNN训练Cifar-10技巧
关于数据集 Cifar-10是由Hinton的两个大弟子Alex Krizhevsky.Ilya Sutskever收集的一个用于普适物体识别的数据集.Cifar是加拿大政府牵头投资的一个先进科学项目 ...
- 详解Oracle数据货场中三种优化:分区、维度和物化视图
转 xiewmang 新浪博客 本文主要介绍了Oracle数据货场中的三种优化:对分区的优化.维度优化和物化视图的优化,并给出了详细的优化代码,希望对您有所帮助. 我们在做数据库的项目时,对数据货场的 ...
随机推荐
- openvswitch 监听端口变化
命令: ovsdb-client monitor Interface name,ofport,external_ids --format=json 运行效果: [root@ostack1 ~]# ov ...
- Kafka Producer源码解析一:整体架构
一.Producer整体架构 Kafka Producer端的架构整体也是一个生产者-消费者模式 Producer线程调用send时,只是将数据序列化后放入对应TopicPartition的Deque ...
- SpringBoot整合Redis并完成工具类
SpringBoot整合Redis的资料很多,但是我只需要整合完成后,可以操作Redis就可以了,所以不需要配合缓存相关的注解使用(如@Cacheable),而且我的系统框架用的日志是log4j,不是 ...
- 区块链入门到实战(38)之Solidity – 条件语句
Solidity支持条件语句,让程序可以根据条件执行不同的操作.条件语句包括: if if...else if...else if 语法 if (条件表达式) { 被执行语句(如果条件为真) } 示例 ...
- 在C++/CLI环境下,千万不要把普通全局函数当标准C/C++的函数指针传递给native的库使用
先上一个简单代码: #include <cstdlib> #include <cstdio> // native apis extern "C" { typ ...
- web前端常见安全问题
1,SQL注入 2,XSS 3,CSRF 4.文件上传漏洞 1,SQL注入:这个比较常见,可能大家也听说过,就是URL里面如果有对数据库进行操作的参数时,要做一下特殊的处理,否则被别有用心的人利用的话 ...
- 10 router
https://router.vuejs.org/zh/guide/advanced/navigation-guards.html 1.路由守卫beforeEach router.beforeEach ...
- Activiti7 流程变量(UEL-Value方式)
需求:请假天数大于3天走总经理审批,小于等于3天直接走人事 画图 因为IDEA不展示那个线上的东西,所以截屏自己写的,还有就是我感觉IDEA画图挺坑的,之前画了好几遍,一部署就报错,很奇怪 /** * ...
- 对Jenkinsfile语法说不,开源项目Jenkins Json Build挺你
对Jenkinsfile语法说不,开源项目Jenkins Json Build挺你 项目背景 我所在的组织项目数量众多,使用的语言和框架也很多,比如Java.ReactNative.C# .NET.A ...
- 判断同名股票是否存在的MyBatis查询函数写法
在A股中,除非股票退市,六位的股票代号是永不变化的,而名称则可能变化,比如更换主业,更换金主,因经营不善而戴帽等,这时名称都会改变. 因此,从网页上爬取的实时股票信息,需要常常与存在本地数据库里的信息 ...