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时刻,存在系统的状态预测方程和系统的状态观测方程:

MSCKF理论推导与代码解析_第1张图片

MSCKF理论推导与代码解析_第2张图片

MSCKF理论推导与代码解析_第3张图片

相比于非线性优化,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的偏置、噪声进行了初值定义:

MSCKF理论推导与代码解析_第4张图片

对于连续时间模型中,IMU的状态更新:

MSCKF理论推导与代码解析_第5张图片

基于上式,IMU误差状态可表示为:

在论文的附录中已给出了F和G的公式,我们在这边进行简单的推导:

MSCKF理论推导与代码解析_第6张图片

其中, 和属于随机噪声,而和是相机和IMU外参的导数,误差不变,导数为0.

IMU采样和的信号,周期为T,在EKF中这些量主要用于状态传播,每次收到新的IMU测量量,均使用IMU状态估计传播方程的五阶/四阶Runge-Kutta积分传播IMU状态估计。同时,EKF的协方差矩阵也被传播。在这里,给出论文中没有详细说明的IMU状态更新,对于IMU状态中的P,V,Q来说,P和V的状态更新是通过Runge-Kutta四阶来进行更新,Runge-Kutta公式详细如下:

MSCKF理论推导与代码解析_第7张图片

对于P和V来说,更新公式如下图:

MSCKF理论推导与代码解析_第8张图片

而Q的更新是假设匀速运动,用角速度与时间相乘:

协方差的更新中,先对协方差矩阵进行划分:

MSCKF理论推导与代码解析_第9张图片

其中,为21*21的IMU状态协方差,为6N*6N的相机位姿估计协方差,是IMU状态误差与位姿估计之间的相关性。由于在一开始已假设所有状态量和噪声都是服从高斯分布,因此只需要关心协方差的更新即可。

协方差更新如下:

MSCKF理论推导与代码解析_第10张图片

在代码开始初始化部分,对进行了初始化预设:

MSCKF理论推导与代码解析_第11张图片

同时对连续噪声协方差Q进行了初始化预设:

量测模型中,根据失去跟踪的特征点建立残差方程:

通过平价空间法将投影至的左零空间,使得:

MSCKF理论推导与代码解析_第12张图片

三、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函数。

MSCKF理论推导与代码解析_第13张图片

双目相机的左右图像传入stereoCallback函数,接着,建立图像金字塔(createImagePyramids()),检测是否是第一张图像,如果是第一张图像,对第一张图像进行图像初始化(initializaFirstFrame()),并提取特征点、进行匹配,通过drawFeaturesStereo()在图像中画出特征点。如果不是第一张图像,首先通过trackFeatures()函数对上一张图像进行特征跟踪,再通过addNewFeatures()加入当前图像中新检测的特征,同样使用drawFeaturesStereo画出图像中的特征点。

Image_process部分的代码较为简单,不用过多分析。接下来主要分析msckf_vio的相关代码,这部分的代码分析和论文较为对应。

和图像处理部分一样,msckf部分从msckf_vio_nodelet.cpp进入initialize(),进入函数后,先进行参数加载,这部分主要有定义坐标系、加载imu的噪声和偏置噪声参数,定义了初始的imu速度,设定了速度协方差、陀螺仪偏置协方差、加速度计偏置协方差、以及外参旋转协方差和外参平移协方差,同时,还定义了整个系统状态协方差的初始化,以及坐标系之间的转换关系。

MSCKF理论推导与代码解析_第14张图片

MSCKF理论推导与代码解析_第15张图片

进入createRosIO(),创建里程计,进行位姿解算。在这一节中,定义两个回调函数,imuCallback和featureCallback,其中,imuCallback的作用仍然是将imu数据输入缓存区间。

featureCallback是整个代码的核心部分,也是最不好理解的部分。

MSCKF理论推导与代码解析_第16张图片

MSCKF理论推导与代码解析_第17张图片

MSCKF理论推导与代码解析_第18张图片

对进行更新后,更新协方差矩阵,先根据论文,定义和

MSCKF理论推导与代码解析_第19张图片

经过化简,可得到:,

MSCKF理论推导与代码解析_第20张图片

MSCKF理论推导与代码解析_第21张图片

根据附录:

MSCKF理论推导与代码解析_第22张图片

扩充方法如下:

MSCKF理论推导与代码解析_第23张图片

扩充后,协方差矩阵的维数将从21*21扩增为27*27,每次扩增增加6维。

以上部分是process model全部内容和代码,接下来,进入量测模型(measurement model),在这部分使用双目相机中观测到的特征位置计算残差。

对应代码removeLostFeatures()函数。由于:

设定为观测到特征点的相机个数,维数为,其左零空间的维数为,因此是的向量。

在featureJacobian函数中定义4*6的,4*3的以及4*1的向量,对应附录中的(measurementJacobian()):

MSCKF理论推导与代码解析_第24张图片

计算出和后,进入measurementUpdate(),进行量测更新,首先计算卡尔曼增益:

MSCKF理论推导与代码解析_第25张图片

然后是更新IMU状态:

更新相机状态:

MSCKF理论推导与代码解析_第26张图片

最后更新状态协方差:

至此,完成了整个系统状态更新。

【结束语】

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理论推导与代码解析)