MSCKF理论推导与代码解析
点击上方“3D视觉工坊”,选择“星标”
干货第一时间送达
在SLAM后端中,主要有两种主流方法用于优化:基于滤波的方法和基于非线性的方法。基于滤波的方法主要有MSCKF、S-MSCKF、ROVIO等,基于非线性的方法主要有OKVIS、VINS-MONO、VINS-Fusion等。在这一节,主要分析S-MSCKF的理论推导和代码解读。
一、扩展卡尔曼滤波(Extended Kalman Filter)
MSCKF的全称是Multi-State Constraint Kalman Filter,意为多约束状态卡尔曼滤波器。那么什么是卡尔曼滤波器(KF)?
通俗来讲,卡尔曼滤波器是根据当前状态,预测估计下一状态的算法。卡尔曼滤波器方法在一定程度上架设了马尔可夫性,也就是k时刻的状态只与k-1时刻的状态有关。卡尔曼滤波器主要解决线性化问题,而将卡尔曼滤波器的结果扩展到非线性系统中,便形成了扩展卡尔曼滤波器(EKF)。
从k-1时刻到k时刻,存在系统的状态预测方程和系统的状态观测方程:
相比于非线性优化,EKF形式更为简洁,应用较广泛,除了EKF外,还有其他滤波器在SLAM中发挥作用,如IF(信息滤波器)、IKF(Iterated KF)、UKF(Unscented KF)和粒子滤波器等。
二、S-MSCKF理论推导与代码分步解读
MSCKF是出自2007年在ICRA会议上发表的A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation,主要是构造了MSCKF系统,针对单目相机实现,但原作者并没有将代码进行开源,github上仍有很多学者进行了仿真,感兴趣可前往github自行寻找。今天分享的这篇是Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight,基于双目的MSCKF,简称S-MSCKF。在单目的基础上扩增了双目摄像头,而且作者进行了代码开源,相比MSCKF更有助于理解论文。
MSCKF设定了两个状态向量:IMU的状态向量和相机的状态向量。IMU的状态向量为:
其中,和分别是陀螺仪和加速度计的偏置,其导数服从高斯白噪声分布。而由于误差表示为:,同时,因此,IMU的误差状态表示为:
k时刻时,假定有N个相机位姿在EKF的状态向量中,则,此时,状态向量表示为:
误差状态向量为:
S-MSCKF代码中,首先对IMU的偏置、噪声进行了初值定义:
对于连续时间模型中,IMU的状态更新:
基于上式,IMU误差状态可表示为:
在论文的附录中已给出了F和G的公式,我们在这边进行简单的推导:
其中, 和属于随机噪声,而和是相机和IMU外参的导数,误差不变,导数为0.
IMU采样和的信号,周期为T,在EKF中这些量主要用于状态传播,每次收到新的IMU测量量,均使用IMU状态估计传播方程的五阶/四阶Runge-Kutta积分传播IMU状态估计。同时,EKF的协方差矩阵也被传播。在这里,给出论文中没有详细说明的IMU状态更新,对于IMU状态中的P,V,Q来说,P和V的状态更新是通过Runge-Kutta四阶来进行更新,Runge-Kutta公式详细如下:
对于P和V来说,更新公式如下图:
而Q的更新是假设匀速运动,用角速度与时间相乘:
协方差的更新中,先对协方差矩阵进行划分:
其中,为21*21的IMU状态协方差,为6N*6N的相机位姿估计协方差,是IMU状态误差与位姿估计之间的相关性。由于在一开始已假设所有状态量和噪声都是服从高斯分布,因此只需要关心协方差的更新即可。
协方差更新如下:
在代码开始初始化部分,对进行了初始化预设:
同时对连续噪声协方差Q进行了初始化预设:
量测模型中,根据失去跟踪的特征点建立残差方程:
通过平价空间法将投影至的左零空间,使得:
三、S-MSCKF代码分析
S-MSCKF的代码分为两部分,图像处理部分(image_processor.cpp)和滤波部分(msckf_vio.cpp)。
【图像处理部分】
图像首先进行初始化操作,从image_processor_nodelet.cpp进入image_processor.cpp
Initialize()函数中先通过loadParameter函数进行相机内外参数的加载,后进入createRosIO(),创建ROS IO,在此函数中,定义两个回调函数:stereoCallback和imuCallback。当有图像传入时,调用stereoCallback函数,进行双目图像的处理,imu数据传入时,调用imuCallback,缓存Imu数据。
主要梳理stereoCallback函数。
双目相机的左右图像传入stereoCallback函数,接着,建立图像金字塔(createImagePyramids()),检测是否是第一张图像,如果是第一张图像,对第一张图像进行图像初始化(initializaFirstFrame()),并提取特征点、进行匹配,通过drawFeaturesStereo()在图像中画出特征点。如果不是第一张图像,首先通过trackFeatures()函数对上一张图像进行特征跟踪,再通过addNewFeatures()加入当前图像中新检测的特征,同样使用drawFeaturesStereo画出图像中的特征点。
Image_process部分的代码较为简单,不用过多分析。接下来主要分析msckf_vio的相关代码,这部分的代码分析和论文较为对应。
和图像处理部分一样,msckf部分从msckf_vio_nodelet.cpp进入initialize(),进入函数后,先进行参数加载,这部分主要有定义坐标系、加载imu的噪声和偏置噪声参数,定义了初始的imu速度,设定了速度协方差、陀螺仪偏置协方差、加速度计偏置协方差、以及外参旋转协方差和外参平移协方差,同时,还定义了整个系统状态协方差的初始化,以及坐标系之间的转换关系。
进入createRosIO(),创建里程计,进行位姿解算。在这一节中,定义两个回调函数,imuCallback和featureCallback,其中,imuCallback的作用仍然是将imu数据输入缓存区间。
featureCallback是整个代码的核心部分,也是最不好理解的部分。
对进行更新后,更新协方差矩阵,先根据论文,定义和
经过化简,可得到:,
根据附录:
扩充方法如下:
扩充后,协方差矩阵的维数将从21*21扩增为27*27,每次扩增增加6维。
以上部分是process model全部内容和代码,接下来,进入量测模型(measurement model),在这部分使用双目相机中观测到的特征位置计算残差。
对应代码removeLostFeatures()函数。由于:
设定为观测到特征点的相机个数,维数为,其左零空间的维数为,因此是的向量。
在featureJacobian函数中定义4*6的,4*3的以及4*1的向量,对应附录中的(measurementJacobian()):
计算出和后,进入measurementUpdate(),进行量测更新,首先计算卡尔曼增益:
然后是更新IMU状态:
更新相机状态:
最后更新状态协方差:
至此,完成了整个系统状态更新。
【结束语】
MSCKF是视觉惯性SLAM上的一个经典里程碑,也有很多工作基于MSCKF做了改进,如2019年CVPR会议上发表的SEVIS,后续也会进行讲解分析。
本文仅做学术分享,如有侵权,请联系删文。
推荐阅读:
专辑|相机标定
专辑|3D点云
专辑|SLAM
专辑|深度学习与自动驾驶
专辑|结构光
专辑|事件相机
专辑|OpenCV学习
专辑|学习资源汇总
专辑|招聘与项目对接
专辑|读书笔记
重磅!3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群,目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群,请扫描下面微信号加群,备注:”研究方向+学校/公司+昵称“,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球:针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近1000+星球成员为创造更好的AI世界共同进步,知识星球入口:
学习3D视觉核心技术,扫描查看介绍,3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
MSCKF理论推导与代码解析相关推荐
- 彻底剖析激光-视觉-IMU-GPS融合SLAM算法:理论推导、代码讲解和实战
应用背景介绍 自主导航是机器人与自动驾驶的核心功能,而SLAM技术是实现自主导航的前提与关键.现有的机器人与自动驾驶车辆往往会安装激光雷达,相机,IMU,GPS等多种模态的传感器,而且已有许多优秀的激 ...
- (01)ORB-SLAM2源码无死角解析-(57) 闭环线程→计算Sim3:理论推导(2)求解R,使用四元数
讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件): (0 ...
- 四面埋伏(车羊问题)代码模拟+理论推导
- 背景: > 你作为选手参加一个名叫<四面埋伏>的战争游戏,获胜奖金额为100万美元.在游戏开始,你模拟一名将军处于游戏场地正中央,在你的4个方向上,有3个方向被设下埋伏,只有一个 ...
- 【计算机视觉】PnP算法简介与代码解析-柴政(solvepnp理论篇)
PnP算法简介与代码解析-柴政 PnP求解算法是指通过多对3D与2D匹配点,在已知或者未知相机内参的情况下,利用最小化重投影误差来求解相机外参的算法.PnP求解算法是SLAM前端位姿跟踪部分中常用的算 ...
- 条件随机场(CRF)极简原理与超详细代码解析
条件随机场(CRF)极简原理与超详细代码解析 1. 原理 1.1 从名称说起 1.2 优化的目标 1.3 如何计算 2. 代码 2.1 基本结构 2.2 模型初始化 2.3 BERT提取的特征如何输入 ...
- CornerNet代码解析——损失函数
CornerNet代码解析--损失函数 文章目录 CornerNet代码解析--损失函数 前言 总体损失 1.Heatmap的损失 2.Embedding的损失 3.Offset的损失 前言 今天要解 ...
- 视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆 本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
- GAN完整理论推导与实现,Perfect!
本文是机器之心第二个 GitHub 实现项目,上一个 GitHub 实现项目为从头开始构建卷积神经网络.在本文中,我们将从原论文出发,借助 Goodfellow 在 NIPS 2016 的演讲和台大李 ...
- 机器学习入门学习笔记:(2.1)线性回归理论推导
理论推导 机器学习所针对的问题有两种:一种是回归,一种是分类.回归是解决连续数据的预测问题,而分类是解决离散数据的预测问题.线性回归是一个典型的回归问题.其实我们在中学时期就接触过,叫最小二乘法. ...
最新文章
- 未能从程序集 XX加载类型XXX的错误解决方法(借以提醒NHibernate使用者)
- ISA2006标准版安装及无人值守安装
- 【整理】Oracle创建/删除表空间
- linux内核匹配的编译器gcc,linux下正则匹配的C实现
- select2 change之前的改变
- EasyJoyStick使用以及两种操作杆 EasyJoyStick的使用方法,简单的不能再简单 Hedgehog Team-》Easy Touch -》Add Easy Touch For C#
- react 绑定 箭头函数_为什么箭头函数和React渲染中的绑定有问题
- boost.asio基础篇 小白入门注解
- 计算机网络课程优秀备考PPT之第一章概述(一)
- 问题十九:怎么模拟ray tracing中漫射材料球体的颜色(diffuse materials)
- Nacos教程_4 配置讲解
- 注册表知识and技巧大全
- java 坦克大战设计论文,java坦克大战毕业设计论文
- SpringCloud 微服务工具集 SpringCloud 版本: Hoxton SR6
- python开发工程师岗位简介_python开发工程师是什么
- Microsoft JDBC Driver XX (XX表示版本号)for SQL Server的安装
- n个元素的全排列(递归+去重)
- 非常棒的开源协同办公OA项目,收藏了!
- 雷达原理习题【西电】
- 机器学习水果识别——python+opencv实现物体特征提取