VINS(四)初始化与相机IMU外参标定
和单目纯视觉的初始化只需要获取R,t和feature的深度不同,VIO的初始化话通常需要标定出所有的关键参数,包括速度,重力方向,feature深度,以及相机IMU外参$R_{c}^{b}$和$p_{c}^{b}$。
一. 外参旋转矩阵初始化
在Feature Detection and Tracking模块中,利用Harris特征点匹配通过基础矩阵和Ransac恢复出$R_{Ck+1}^{Ck}$;
相应的使用IMU陀螺仪数据积分得到$R_{bk+1}^{bk}$;
这两个测量满足:
$R_{bk+1}^{bk}\cdot R_{c}^{b} = R_{c}^{b}\cdot R_{Ck+1}^{Ck}$
将测量得到的$R$用四元数表示,并利用四元数的运算(左乘右乘),可以得到一个约束方程:
$Q_{k+1}^{k}\cdot q_{c}^{b} = 0$
利用多组数据可以获得一个超定方程:
$Q_{N}\cdot q_{c}^{b} = 0$
通过求解最小二乘解,得到$q_{c}^{b}$。注意其中$Q_{N}$是包含使用Huber核的权重的,权重是通过残差矩阵计算得到,权重通过计算IMU项和视觉findFundamentalMat计算得到的旋转矩阵之间的差别得到,差别越大的$Q_{k+1}^{k}$,在$Q_{N}$中权重越小。
旋转初始化成功判定依据:
如果IMU三轴充分旋转,那么4*4矩阵$Q_{N}$的null space零空间的秩为1,可以得到稳定解;
如果旋转退化,则$Q_{N}$零空间的秩大于1,使用SVD分解,判断倒数第二小(第三大)的奇异值是否大于阈值。
因此,旋转标定初始化的关键在有充分的三轴旋转。
二. 速度,重力方向,feature深度,以及外参平移矩阵初始化
可以将$R_{c}^{b}$,$R_{bk+1}^{bk}$,$R_{bk+1}^{b0}$当做已知固定值,使用最大似然估计(Maximum Likelihood Estimation, MLE)和Mahalanobis范数(可以理解为带权重的二范数(椭球向量范数))构建cost function,构建无偏的状态估计问题。
在Sliding Window(例如选取5帧)中对测量值构建最小二乘问题:
VINS中feature没有通过描述子匹配跟踪,也不仅像ORB-SLAM中选取观测次数多(跟踪效果好)的特征点,另外加入了视角足够大这个条件。虽然最后选取出的特征点数量比较少(150个左右),但是都是可观性很好的特征点。
(一)IMU测量模型
构建目标函数需要对测量模型建模,IMU的测量模型和预积分一样,预积分得到的测量值$\hat{z}$与状态估计值之间的差值取Mahalanobis范数,其中权重为协方差矩阵的逆(信息矩阵)。
参考预积分公式:
得到IMU测量的误差方程:
其中
为了求解高斯分布的最大似然,需要知道测量的协方差及其对应的状态转移方程(用于协方差的更新)
通过IMU的运动学方程微分,可以获得状态转移方程
假设初始IMU协方差$P_{bk}^{bk}$为0,则协方差传播方程为
协方差矩阵的传播和卡尔曼滤波的预测过程中协方差的传播是一样的,从系统的微分运动方程获取的。VINS中使用的是first-order discrete-time propagation,或者称为forward Euler method,其中加速度计的噪声$Q_{t}$需要预先知道或者使用imu_tk标定。
(二)视觉测量模型
重投影误差在VIO中,通常需要经过IMU坐标系的变换;这样有两个好处:
- 将IMU的参数(外参)整合在视觉的residual中一起参与优化;
- IMU的坐标系在预积分中是已知的,不需要再去计算;
因此将i帧获取的2D feature重投影到j帧上满足观测方程:
其中的坐标变换如下图所示:
初始化收敛判定:
cost function的协方差矩阵中外参平移向量$p_{c}^{b}$对应的协方差值(SVD分解)小于设定阈值时,收敛,初始化完成。由于要获得协方差矩阵(对应整个状态向量,而不是每一块误差项),需要对信息矩阵求逆,计算量较大,因此会放在新的线程中去完成(这里似乎有可以优化的空间)。
VINS(四)初始化与相机IMU外参标定的更多相关文章
- 相机imu外参标定
1. 第一步初始化imu外参(可以从参数文档中读取,也可以计算出),VINS中处理如下: # Extrinsic parameter between IMU and Camera. estimate_ ...
- 相机-imu外参校准总结
1. 研究背景及相关工作 1)研究背景 单目视觉惯性slam是一种旨在跟踪移动平台的增量运动并使用来自单个车载摄像头和imu传感器的测量结果同时构建周围环境地图的技术.视觉相机和惯性测量单元(imu) ...
- 解放双手——相机与IMU外参的在线标定
本文作者 沈玥伶,公众号:计算机视觉life,编辑部成员 一.相机与IMU的融合 在SLAM的众多传感器解决方案中,相机与IMU的融合被认为具有很大的潜力实现低成本且高精度的定位与建图.这是因为这两个 ...
- 相机IMU融合四部曲(三):MSF详细解读与使用
相机IMU融合四部曲(三):MSF详细解读与使用 极品巧克力 前言 通过前两篇文章,<D-LG-EKF详细解读>和<误差状态四元数详细解读>,已经把相机和IMU融合的理论全部都 ...
- 相机IMU融合四部曲(一):D-LG-EKF详细解读
相机IMU融合四部曲(一):D-LG-EKF详细解读 极品巧克力 前言 前两篇文章<Google Cardbord的九轴融合算法>,<Madgwick算法详细解读>,讨论的都是 ...
- 【项目实战】自备相机+IMU跑通Vins-Mono记录
前言 初次接触SLAM,公司要求用自己的设备来跑通vinsmono这个程序,虽然已经跑通了别人的数据包,但是真正自己上手来运行这个程序,发现真的是困难重重,特意在此记载下来整个过程,以供大家参考. 我 ...
- 相机IMU融合四部曲(二):误差状态四元数详细解读
相机IMU融合四部曲(二):误差状态四元数详细解读 极品巧克力 前言 上一篇文章,<D-LG-EKF详细解读>中,讲了理论上的SE3上相机和IMU融合的思想.但是,还没有涉及到实际的操作, ...
- Spring IOC容器启动流程源码解析(四)——初始化单实例bean阶段
目录 1. 引言 2. 初始化bean的入口 3 尝试从当前容器及其父容器的缓存中获取bean 3.1 获取真正的beanName 3.2 尝试从当前容器的缓存中获取bean 3.3 从父容器中查找b ...
- SQL的四种连接(左外连接、右外连接、内连接、全连接)
1.内联接(典型的联接运算,使用像 = 或 <> 之类的比较运算符).包括相等联接和自然联接. 内联接使用比较运算符根据每个表共有的列的值匹配两个表中的行.例如,检索 stude ...
随机推荐
- WebSphere Studio Application Developer 5.0 优化设置
公司有一个项目需要用到WebSphere Studio Application Developer 5.0 的开发环境,这个环境比较老,而且只能用JDK1.4. 项目开发的时候 总是报错: JVM t ...
- nbu异地备份实施前,数据收集日志
1.修改bp.conf配置文件显示重删率 BPDBJOBS_COLDEFS = JOBID 5 true BPDBJOBS_COLDEFS = TYPE 7 false BPDBJOBS_COLDEF ...
- vue通过watch对input做字数限定
<div id="app"> <input type="text" v-model="items.text" ref=&q ...
- if not
if not x 在python中的意思是如果x为空 a = [] if not a: print 1 此代码会打印出1 a = [1,2,3] if not a: print 1 此代码不会打印出1 ...
- phpstorm下TODO注释
TODO注释 什么是TODO注释? 标记的注释,表示你代办的任务. 作用 标记你需要编写的任务位置 使用方法 TODO: + 说明: 如果代码中有该标识,说明在标识处有功能代码待编写,待实现的功能在说 ...
- 01-Python学习笔记-基础语法
Python标识符 -d 在解析时显示调试信息 -O 生成优化代码 ( .pyo 文件 ) -S 启动时不引入查找Python路径的位置 - ...
- sqlplus常用的几种登录方式
1. sqlplus / as sysdba 操作系统认证,sys管理员登录,/后面要有空格. 2. sqlplus "/ as sysdba" 操作系统认证,sys管理员登录,/ ...
- iOS 类似美团或饿了么评价中的星星评分控件
1.做的好几个项目都用到了评分控件,可以用来展示评分,也可以用来写评分,图片和间距大小都可以定制,之前就已经简单封装了一个,现在把它分享出来,有需要的拿去用. 2.下面是展示截图: image.p ...
- iOS开发学习资源
最近想写点关于iOS开发的总结和心得.虽然网上资源一大堆,质量参差不齐,还是推荐一点干货吧! https://www.objc.io/issues/ 这个网站的文章质量很高,很多干货,可惜今年已经停 ...
- 竞赛题解 - Karp-de-Chant Number(BZOJ-4922)
Karp-de-Chant Number(BZOJ-4922) - 竞赛题解 进行了一次DP的练习,选几道题写一下博客~ 标签:BZOJ / 01背包 / 贪心 『题目』 >> There ...