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的状态:

\[\mathbf{x}_{I}=\left(\begin{array}{cccccccc}
^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}\)

\[\delta \mathbf{q} \approx\left(\frac{1}{2} G \tilde{\boldsymbol{\theta}}^{\top} \quad 1\right)^{\top}
\]

这里 \(^G_I \tilde{\theta} \in R^3\) 表示小旋转.

最终, N 个相机状态如下:

\[\tilde{\mathbf{x}}=\left(\begin{array}{cccc}
\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的定义是:

\[\tilde{\mathbf{x}}_{C_{i}}=\left(\begin{array}{cc}
C_{i} & \tilde{\boldsymbol{\theta}}^{\top}
\end{array}^{G} \tilde{\mathbf{p}}_{C_{i}}^{\top}\right)^{\top}
\]

为了保持计算的复杂度, 雨鞋相机状态需要被边缘化, 当相机的状态量到一个阈值到时候.

A. Process Model

估计的IMU状态的continues dynamics:

\[\begin{array}{c}
{ }_{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除去偏置的测量

\[\hat{\omega} = \omega_m - \hat{b_g}, \hat{a} = a_m - \hat{b_a}
\]

同时:

\[\Omega(\hat{\omega})=\left(\begin{array}{cc}
-\left[\hat{\omega}_{\times}\right] & \omega \\
-\omega^{\top} & 0
\end{array}\right)
\]

linearized continuous dynamics 对于 error IMU state:

\[\dot{\tilde{\mathbf{x}}}_{I}=\mathbf{F} \tilde{\mathbf{x}}_{I}+\mathbf{G} \mathbf{n}_{I}\tag2
\]

这里 \(\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)的离散时间状态转换矩阵和离散时间噪声协方差需要先计算.

\[\begin{aligned}
\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+1 \mid k}}=\boldsymbol{\Phi}_{k} \mathbf{P}_{I I_{k \mid k}} \boldsymbol{\Phi}_{k}^{\top}+\mathbf{Q}_{k}
\]

整个状态的协方差是:

\[\mathbf{P}_{k \mid k}=\left(\begin{array}{cc}
\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}_{k+1 \mid k}=\left(\begin{array}{cc}
\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状态获得

\[G \hat{\mathbf{q}}={ }_{I}^{C} \hat{\mathbf{q}} \otimes_{G}^{I} \hat{\mathbf{q}}, \quad{G}_{\hat{\mathbf{p}} C}={ }^{G} \hat{\mathbf{p}}_{C}+C\left({ }_{G}^{I} \hat{\mathbf{q}}\right)^{\top}{ }^{I} \hat{\mathbf{p}}_{C}
\]

增广的协方差就是:

\[\mathbf{P}_{k \mid k}=\left(\begin{array}{c}
\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\)可以表示为:

\[\mathbf{z}_{i}^{j}=\left(\begin{array}{c}u_{i, 1}^{j} \\ v_{i, 1}^{j} \\ u_{i, 2}^{j} \\ v_{i, 2}^{j}\end{array}\right)=\left(\begin{array}{cc}\frac{1}{C_{i, 1} Z_{j}} & \mathbf{0}_{2 \times 2} \\ \mathbf{0}_{2 \times 2} & \frac{1}{C_{i, 2} Z_{j}}\end{array}\right)\left(\begin{array}{c}C_{i, 1} X_{j} \\ C_{i, 1} Y_{j} \\ C_{i, 2} X_{j} \\ C_{i, 2} Y_{j}\end{array}\right)
\]

注意我们可以让\(z\) 是 \(R^3\), 但是要rectified. 如果是 \(R^4\) 的话, 就不需要同一个特征的观测要在一个平面上了.

这个测量的残差近似为:

\[\mathbf{r}_{i}^{j}=\mathbf{z}_{i}^{j}-\hat{\mathbf{z}}_{i}^{j}=\mathbf{H}_{C i}^{j} \tilde{\mathbf{x}}_{C_{i}}+\mathbf{H}_{f_{i}}^{j} \overline{\mathbf{p}}_{j}+\mathbf{n}_{i}^{j}
\]

这里 \(n_i^j\) 是测量误差. 两个 \(H\) 是jacobian.

通过积累一个特征\(f_j\) 的多个观测, 我们有:

\[\mathbf{r}^{j}=\mathbf{H}_{\mathbf{x}}^{j} \tilde{\mathbf{x}}+\mathbf{H}_{f}^{j} G \tilde{\mathbf{p}}_{j}+\mathbf{n}^{j}
\]

如[1]中所述, 因为\(^Gp_j\) 是用相机位姿计算拿到, 它的不确定性也是和状态中的相机位姿相关的.

为了保证它的不确定性不影响残差, Eq 5是投影到零空间的.

\[\mathbf{r}_{o}^{j}=\mathbf{V}^{\top} \mathbf{r}^{j}=\mathbf{V}^{\top} \mathbf{H}_{\mathbf{x}}^{j} \tilde{\mathbf{x}}+\mathbf{V}^{\top} \mathbf{n}^{j}=\mathbf{H}_{\mathbf{x}, o}^{j} \tilde{\mathbf{x}}+\mathbf{n}_{o}^{j} \tag6
\]

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的更多相关文章

  1. 一步一步学习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)} ...

  2. 83 项开源视觉 SLAM 方案够你用了吗?

    作者:吴艳敏 来源:83 项开源视觉 SLAM 方案够你用了吗? 前言 1. 本文由知乎作者小吴同学同步发布于https://zhuanlan.zhihu.com/p/115599978/并持续更新. ...

  3. MSCKF_VIO:MSCKF的双目版本

    论文:MSCKF的双目版本 Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight 下载地址:点击 源码地址:https:/ ...

  4. ROSCon 2016视频和幻灯片发布 ROS机器人操作系统重要参考资料

    ROSCon 2016视频和幻灯片发布 By Tully Foote on 十月19,2016 7:28 AM 全部PPT下载地址:http://pan.baidu.com/s/1gf2sn2F RO ...

  5. 计算机视觉code与软件

    Research Code A rational methodology for lossy compression - REWIC is a software-based implementatio ...

  6. SVO+PL-SVO+PL-StVO

    PL-SVO是基于点.线特征的半直接法单目视觉里程计,我们先来介绍一下基于点特征的SVO,因为是在这个基础上提出的. [1]References:      SVO: Fast Semi-Direct ...

  7. APM程序分析-ArduCopter.cpp

    该文件是APM的主文件. #define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Copter, &copter ...

  8. CNN训练Cifar-10技巧

    关于数据集 Cifar-10是由Hinton的两个大弟子Alex Krizhevsky.Ilya Sutskever收集的一个用于普适物体识别的数据集.Cifar是加拿大政府牵头投资的一个先进科学项目 ...

  9. 详解Oracle数据货场中三种优化:分区、维度和物化视图

    转 xiewmang 新浪博客 本文主要介绍了Oracle数据货场中的三种优化:对分区的优化.维度优化和物化视图的优化,并给出了详细的优化代码,希望对您有所帮助. 我们在做数据库的项目时,对数据货场的 ...

随机推荐

  1. nova start 虚机的代码流程分析

    nova start 虚机的代码流程分析,以ocata版本为分析基础1.nova api服务接受用户下发的 nova start启动虚机请求其对应的http restfull api接口为post / ...

  2. 手把手教Linux驱动2-之模块参数和符号导出

    通过<手把手教Linux驱动1-模块化编程,玩转module>的学习,我们已经掌握了如何向内核加载一个模块,现在我们学习模块之间如何传递参数. 一.给模块传递参数 当我们加载一个模块到Li ...

  3. 为什么我们需要Logstash,Fluentd等日志摄取器?

    前文传送门:Logging with ElasticSearch, Kibana, ASP.NET Core and Docker 疑问:既然应用能直接向ElasticSearch写日志,为什么我们还 ...

  4. Linux高频操作

    常用linux命令 查看文件.文件夹大小 du -hs <file/dir> 同时输出到控制台和文件 ./<program> 2>&1 | tee <log ...

  5. 高并发&性能优化(一)------总体介绍

    [开篇词] 本文主要通过一些经典的高并发场景,以及一些基本的运维工具来讲述一些关于高并发以及性能优化相关的内容,主要包括性能瓶颈的定位,性能调优的思路和技巧等. [性能的衡量指标] ?什么是性能 性能 ...

  6. Answers for Q1 and Q2

    A1: 1. enetity-data model mapping: 2. database design 2.1  sql create table A_manufacturer_info(manu ...

  7. 创建human用户登录数据库创建表

    根据人力资源管理系统中表的设计,创建human用户登录数据库创建 准备阶段 把运行脚本复制到D:\app\Administrator\product\11.2.0\dbhome_1\demo\sche ...

  8. HTTP系列之跨域资源共享机制(CORS)介绍

    前言 本文将继续解析详解HTTP系列1中的请求/ 响应报文的首部字段,今天带来的跨域资源共享(CORS)机制,具体内容包括CORS的原理.流程.实战,希望能给大家带来收获! CORS简介 跨域资源共享 ...

  9. [CSP-S2019]Emiya 家今天的饭 题解

    CSP-S2 2019 D2T1 很不错的一题DP,通过这道题学到了很多. 身为一个对DP一窍不通的蒟蒻,在考场上还挣扎了1h来推式子,居然还有几次几乎推出正解,然而最后还是只能打个32分的暴搜滚粗 ...

  10. 详细分析栈和队列的数据结构的实现过程(Java 实现)

    目录 栈和队列的数据结构的实现过程(Java 实现) 栈的数据结构的实现 栈的基础知识回顾 栈的常见应用 基于数组的栈的实现 具体代码设计 基于数组的栈简单的时间复杂度分析 关于栈的一个算法应用:括号 ...