VINS-Mono:一种稳健的单目视觉惯性状态估计器_danmeng8068的博客-程序员信息网

技术标签: slam  

编者序:本文提出的紧耦合滑窗VINS状态估计算法,可在未知状态情况下稳健初始化,具有线外参标定、统一定义在球面上的重投影误差、闭环检测和4自由度位姿图优化的特点。本文是香港科技大学沈邵劼课题组的开源杰作,媲美state-of-the-art。将分为上、下两部分介绍,上部分侧重整体及实验,下部分侧重理论细节及应用。

推荐指数☆☆☆☆☆

一、引言

本文提出的视觉惯性系统结构框图如下所示。第一部分是测量处理前段,负责提取并跟踪每个新图像帧的特征,及帧间IMU数据预积分。第二部分是初始化过程,提供了激活非线性系统的必要初始值,如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。第三部分为非线性图优化,通过联合优化所有的视觉、惯性信息求解滑窗内的状态。另一部分运行另一个线程,负责闭环检测及姿态图优化。

主要分为五部分

1. 传感器数据处理:

  • 单目相机Monocular Camera: Feature detection and Tracking
  • IMU: Pre-integration

2. 初始化:

  • 仅使用视觉构建SfM
  • 将SfM结果和IMU预积分结果对齐

3. 基于滑动窗口的非线性优化:

4. 闭环检测:

5. 4自由度全局位姿图优化:


二、各部分概述

测量预处理:对于视觉测量,跟踪连续帧间的特征并检测最近一帧中的新特征;对于IMU测量,连续帧内进行预积分。注意IMU测量受偏差和噪声的影响,因此我们在预积分和优化部分考虑了偏差,这对低成本IMU非常必要。

初始化:单目紧耦合视觉惯性系统是高度非线性系统。由于单目相机不能直接观测到尺度,因此没有好的初始值情况下很难直接融合这两种传感器。一般的,在静止假设的前提下,认为开始几秒的IMU测量值平均是重力向量,IMU传播(propagation)认为是初始姿态。然而,当IMU测量值受非平凡(non-trivial)偏差影响或开始时存在加速运动情况时,这么认为是不合适的。为提高单目视觉惯性系统的成功率,需要稳健的初始化。我们采用松耦合传感器融合方法获得初始值。我们发现仅视觉SLAM或SfM初始化性能良好,大多数情况下可通过Eight-point、Five-point、homogeneous and fundamental等相对运动方法完成初始化。通过将IMU预积分结果与仅视觉对其,可以大体上获得尺度、重力、速度,甚至偏差,这对激活非线性系统是很有好处的,如下图所示。

紧耦合非线性优化:状态初始化完成后,运行基于滑窗的非线性估计器进行高精度状态估计,使用Ceres Solver 求解该非线性优化问题。滑窗说明如下图所示。

闭环检测:我们在一个单独线程里进行闭环检测和维持一个仅姿态图。由于滑窗缺少绝对位置和偏航观测,当检测到闭环时,滑窗将通过重定位贴在姿态图上。关键帧从滑窗中边缘化后将被加入到姿态图,当新加入的关键帧包含闭环信息时执行姿态图优化。IMU测量提供了完整的俯仰角和滚动角,所以仅xyz和偏航角4个自由度上有累积漂移,为避免引入虚假信息,我们直接在这4个自由度上优化姿态图。

三、实验

3.1 数据集对比

算法评估使用的是EuRoc MAV数据集,该数据集利用无人机搭载的双目相机和IMU采集的,并通过VICON和Leica MS50采集ground truth。我们仅仅利用双目左边的相机数据。该数据集包括非平凡IMU偏出和光照变化,非常有代表性有挑战性。本实验选用MH_03_median和MH_05_difficult数据。

对比算法选的是目前state-of-the-art算法OKVIS,该算法是另一种基于优化的滑窗算法。与OKVIS算法相比,我们的算法在稳健初始化和闭环检测上更完整,在每个细节上都不太一样。为描述简单起见,记VINS为仅里程计,VINS_loop为含闭环检测姿态图优化的本文算法,OKVIS_mono和OKVIS_stereo分别为单目和双目OKVIS算法。为公平起见,我们丢掉开始的100个输出,取接下来的150个输出,计算该输出到groud truth的变换并对其,由于单目系统需要更长的时间收敛。

实验结果如下图所示。对平移误差来讲,VINS_loop误差最小。对旋转误差来讲,OKVIS更稳定些,在俯仰角和滚动角估计上更准确,可能的原因是本文算法为节约计算资源使用了IMU一阶近似去做预积分,或者OKVIS以更高的频率边缘化IMU约束。

另外,对于仅里程计,本文算法和OKVIS都很好,难分高下,且基于优化的紧耦合算法将来也并不能大幅度提高精度。

3.2 室内环境

在实验室手持设备来回走,经历了行人、黑暗、低特征区域、镜面反射等,代表了典型的日常生活。所用设备及实验结果如下图所示。

3.3 大规模环境

从室内环境转入室外环境:从实验室座位上出发走过实验室、下楼梯去室外到处走走,返回实验室并爬楼梯,回到座位上,整个轨迹10分钟700多米。不用闭环检测时,起点和终点误差距离是[-5.47,2.76,-0.29],约0.88%;带闭环检测是误差距离为[-0.032,0.09,-0.07],可忽略。轨迹及与Google earth对齐结果如下图所示。

大规模实验在HKUST校园进行,约710米长,240米宽,60米高度变化。整个轨迹持续1小时34分钟,共5.62km,可实验系统稳定性和持久性。设置关键帧数据库大小为2000,兼具是实现和足够的闭环信息。每当检测到闭环时,优化所有的姿态图。在主频为3.6Hz的Intel i7-4790 cpu上耗时统计及与Google地图比较结果如下图所示。

英文名称:VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

链接及源码:

https://github.com/HKUST-Aerial-Robotics/VINS-Mono/blob/master/support_files/paper/tro_technical_report.pdf



一、总体框架回顾

本文提出的视觉惯性系统结构框图如下所示。第一部分是测量处理前端,负责提取并跟踪每个新图像帧的特征,及帧间IMU数据预积分。第二部分是初始化过程,提供了激活非线性系统的必要初始值,如姿态、速度、重力向量、陀螺仪偏差及3D特征位置。第三部分为非线性图优化,通过联合优化所有的视觉、惯性信息求解滑窗内的状态。另一部分运行另一个线程,负责闭环检测及姿态图优化。

二、测量预处理

2.1视觉预处理前端:

对每张新图像,使用KLT稀疏光流算法跟踪已有特征,同时检测新的角点特征以维持每张图像100-300特征。检测器通过设置相邻特征最小间隔强制特征均匀分布,outlier去除后,特征投影在单位球面上(unit sphere)。Outlier通过RANSAC基础矩阵测试去除。

关键帧的选取原则有两个:平均视差和跟踪的特征数。当跟踪的特征数小于某个门限或者跟踪特征的平均视差超过某个门限时,插入关键帧。请记住,除了平移,旋转也会导致视差,但是纯旋转时特征无法三角定位,我们在计算视差时用IMU propagation结果补偿旋转。

2.2 IMU预积分

对于基于优化的算法来说,IMU预积分可节约大量的计算资源,因为不需要一遍遍计算IMU测量。IMU预积分部分,仅仅与IMU偏差有关,当偏差变化小时,可通过一阶近似调整,否则才需要重新计算。离散时间预积分可通过Euler、Mid-point、RK4等数值计算方法计算。我们程序中采用内点法计算。

经推导整理,IMU测量模型可写成如下形式

三、初始化

3.1 滑动窗口视觉SfM

初始化时,需要先执行视觉SfM,估计up-to-scale相机位姿和特征位置。首先检查最后一帧和前面帧的特征匹配,如果找到多余30个跟踪特征并且平均视差对于20个像素的前面帧,则通过5点法恢复这两帧间的相对旋转和up-to-scale平移,否则将最后一帧留在滑动窗口并等待下一帧。如果5点法成功了,则固定平移的scale并三角化这两帧的所有特征。基于这些三角化的特征,使用PnP(Perspective-n-Point)算法n-Point估计窗口内其余帧的位姿。最后执行全局BA最小化所有特征的重投影误差。由于绝对尺度未知,可用第一帧作为参考帧,其余帧的位姿和特征位置用第一帧表示。

3.2 视觉惯性对齐

首先根据SfM估计出的相邻两帧间的旋转和IMU预积分得到的约束,通过将IMU预积分线性化为陀螺仪偏差的函数,通过最小化窗口内所有帧的旋转误差;计算出陀螺仪偏差。然后构建并求解最小二乘问题得到速度、重力向量和尺度的初始估计;进而通过约束重力向量的幅度,在tangent空间求解出重力向量精确值(尤其是重力方向),如下图所示。最后根据旋转矩阵调整速度和尺度,完成初始化。

四、紧耦合非线性优化

4.1 公式

优化公式中包含窗口内的所有关键帧(参数有平移、旋转、速度、加速度偏差和陀螺仪偏差)和所有特征(特征的inverse depth)。优化的损失函数是边缘化的先验信息和视觉与IMU误差的Mahalanobis范数之和,如下所示。

其中IMU剩余误差和视觉剩余误差依次s如下图所示。

我们在边缘化IMU状态和图像特征的同时,根据边缘化状态将测量加入到先验信息。边缘化策略如下图所示。当倒数第二帧是关键帧时,将最早一帧及其相应测量边缘化;如果倒数第二帧不是关键帧,我们仅仅将倒数第二帧的视觉测量扔掉而保留它的IMU测量。该策略增加了系统的稀疏性。边缘化不仅可以保持窗口内的关键帧数量恒定,而且限定了预积分IMU测量的不确定性。我们使用Schur消元实现边缘化,通过边缘化,去掉状态的重要信息得以保留且计算复杂度恒定(important information of the removed states is kept and computation complexity is bounded)。

由于光照强烈变化或剧烈运动,失败不可避免。主动监测错误并从错误中恢复可以增加系统的实用性。错误检测的原则有:跟踪的特征数或最新一帧特征数少于5、最后两个输出的位置或旋转有突变、偏差或外参估计有很大的值。检测到错误后,以最后可能的位姿,重新初始化系统。同时,保留关键帧数据库,用于闭环检测。

五、闭环检测

5.1 闭环检测

我们使用DBoW2词袋模型和BRIEF描述子检测闭环。当新插入关键帧时,视觉前端提取的特征并不够,需要额外提取500个FAST角点特征,并计算所有特征的BRIEF描述子。检测到闭环时,执行时空一致性检查后,DBoW2返回候选关键帧,将原始图像扔掉而保留BRIEF描述子,以节省内存。

5.2 特征检索

当检测到闭环时,需要检索特征建立连接,本文仅建立含深度信息的前端提取的特征间的连接。直接匹配会有很多outlier,我们限定了匹配区域,每个特征仅在对应位置邻域内搜索。然后使用RANSAC检测几何一致性移除outlier。当匹配的特征数超过一定门限时,认为是正确的闭环匹配,融合二者信息,如下图所示。

5.3 重定位

由于新帧和窗口内的其它关键帧有很多相同的特征,因此很容易将闭环帧与窗口内关键帧进行联合优化,加入闭环帧仅增加了视觉约束而没有增加IMU约束,如下所示。

5.4 位姿图加入关键帧

当关键帧被边缘化时,将它加入位姿图,实际上为减少计算量每三个才加入一个。这些关键帧是位姿图的顶点,位姿图的边分为顺序边和闭环边两种。

5.5 4DOF位姿图优化

由于惯性视觉系统可以观测到绝对尺度、俯仰角和滚动角,累积漂移仅出现在位置xyz和偏航角4个自由度上。因此闭环后,需要进行4自由度而不是6或7自由度位姿图优化。其中闭环边的优化函数选huber稳健损失函数,如下所示。

5.6 数据库管理

随着轨迹的增加,数据库变得越来越大,闭环检测和位姿图优化时间越来越长,长时间实时应用困难。我们限定数据库大小恒定,当关键帧数量超过某个门限时,根据关键帧密度分布去掉一些密集的关键帧,从而避免上述问题。

六 应用

无人机平台及IPhone7手机上实验结果如下图所示。其中手机平台是与tango做对比,无人机平台用的是鱼眼相机。


版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/danmeng8068/article/details/79485085

智能推荐

【CI3.1】CI框架简单使用方法_weixin_30652897的博客-程序员信息网

CI框架简单使用方法1、回忆MVC 1.1、M:模型,提供数据,保存数据1.2、V:视图,只负责显示,表单form1.3、C:控制器,协调模型和视图1.4、action:动作,是控制器中的方法,用于被浏览器请求 2、CI中的MVCCI简介:CodeIgniter是一个轻量级但功能强大的php框架,基于MVC的设计模式。提供...

为什么说SQL语句中使用IN性能不高?_左直拳的博客-程序员信息网_sql in的效率

因为本质上in就是多个=,用or连接起来。同事写了一条SQL语句,我一看,哦买糕的,居然用了4层嵌套的in。SELECT * FROM "publicDB"."system_action" WHERE ACTION_ID IN( SELECT ACTION_ID FROM "publicDB"."system_action_role" WHERE ROLE_ID IN ( SELECT ROLE_ID FROM "publicDB".base_role_user

笔记本显示网络电缆被拔出怎么解决_我的电脑本地连接已连接但显示网络电缆被拔出是怎么回事啊..._weixin_39594895的博客-程序员信息网

我的电脑本地连接已连接但显示网络电缆被拔出是怎么回事啊以下文字资料是由(历史新知网www.lishixinzhi.com)小编为大家搜集整理后发布的内容,让我们赶快一起来看一下吧!我的电脑本地连接已连接但显示网络电缆被拔出是怎么回事啊解决方法:1,先换下路由器跟电脑的网线接口,看看是不是路由器现在用的这个网线接口坏了.2,看看电脑的网线接口有么有没插紧,或者里面塞了东西,灰尘多一类的.3,换一根网...

SpringBoot的幕后推手,Java后端知识体系_普通网友的博客-程序员信息网

阶段二、分析和组装第一个阶段完成以后,我们可以先暂时认为IoC容器中保存着一个个相互独立的bean,它们之间还没有任何关系,但是实际项目中它们之间是有着不可或缺的关系的,所以呢,Ioc容器第二个阶段需要做的工作就是分析这些已经在IoC容器中bean,根据它们的依赖关系先后按顺序组装它们,工作原来是这样的:IoC发现一个bean依赖另外一个bean,那么它会将另一个bean注入给依赖它的那个bean,一直到所有的bean的依赖都完成注入。这个时候容器中所有的bean都已经准备好待使用,也就标志着整个IoC容

java连续输入字符串遇到回车停止_lls60s的博客-程序员信息网

连续输入字符串用空格隔开,遇到回车停止用split()方法,将字符串用分隔符进行分离split 分离;使分离public String[] split(String regex)参数为String型。在参数位置设置分隔符,例如:“/”(斜杠) ,“ ”(空格) ,“|”(竖杠) 等。返回值类型为String[] 字符串数组。将第一个分隔符左边的字符存入下标为0的字符串,第一个分隔符...

【better-scroll】better-scroll移动端的滚动组件(不会出现滚动条)_meichaoWen的博客-程序员信息网

github地址:https://github.com/ustbhuangyi/better-scroll当 better-scroll 遇见 Vue:https://zhuanlan.zhihu.com/p/27407024一、安装npm install better-scroll --save二、在Vue项目中应用<template> <div class="wrapper" ref="wrapper"> <ul class="cont

随便推点

PCB_<3>DIP16封装的绘制_weixin_30316097的博客-程序员信息网

任务二 DIP16封装的绘制对于标准的PCB封装元器件封装,Altium Designer为用户提供了【元器件封装向导】,帮助用户完成PCB封装的制作。使用元器件封装向导,设计者只需按照提示进行一系列设置后,就可以建立一个器件封装。下面将绘制DIP16的封装。DIP的外形尺寸和引脚间距如图所示结果如下第1步:创建一个pcb工程,还有...

ActiViz(VTK的C#库)学习使用心得之二:Activiz.NET的下载和安装_chengzhan3748的博客-程序员信息网

一、ActiViz的下载 建议到Kitware公司的主页去下载ActiViz(https://www.kitware.eu/product/activiz),在Kitware的ActiViz产品页面,我们可以找到如下下载栏。 需要说明的几个问题: 1.关于安装包内容:...

HTML及HTML基础标签_忧郁的梧桐树叶的博客-程序员信息网_hhtmlyhttttvtttttttpchltt

HTMLHTML全称是超文本标记语言,英文全称为:HyperText Markup Language。HTML的整体框架包括顶层标签()、头部标签()、体标签()。html的各种标签均为尖括号,一般情况标签都是成对出现,如果标签没有内容的话,则会出现单个,并且标签会有结尾符,如:。html使用形式来进行注释,标签之间可以嵌套,但不可以交错,注释标签之内不可以嵌套注释标签。标签不区分大小...

JAVA文件上传和下载_BurNing_H的博客-程序员信息网

实现WEB开发中的文件上传功能,需完成如下二步操作:在WEB页面中添加上传输入项,,使用时注意:1.          必须要设置input输入项的name属性,否则浏览器将不会发送上传文件的数据。2.          必须把input项的enctype属值设为multipart/form-data.设置该值后,浏览器在上传文件时,将把文件数据附带在http请求消息体中,

贴图教程_次世代游戏基础教程-maya烘焙AO贴图_拾一带你学编程的博客-程序员信息网

MAYA  AO(Ambient Occlusion)烘焙AO就是Ambient Occlusion的缩写,直接翻译中文是环境闭塞,是CG动画和游戏制作常用的一个制作流程。简单地说就是通过运算,渲染并记录下物体的全局光影。有了它,可以渲染并记录物体的凹凸,上下的光影变化信息,对于贴图的制作很有帮助,一般在贴图上叠加一张AO图,会使作品显得更加厚重有力,物体的暗部更有细节。在MAYA里,我们通常用它...

Hp跟踪进程的工具tusc_cuininmi5584的博客-程序员信息网

今天在分析oracle startup 过程中hung的问题时,hp工程师使用了tusc工具,尝试后感觉很好,并作搜集如下:http://h20331.www2.hp.com/hpsub/cache/286022-0-0-2...

推荐文章

热门文章

相关标签