点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
Sliding Windows Filter（SWF）在VIO、SLAM这个领域应用非常广，比如MSCKF、OKVIS、VINS-Mono等等，几乎可以说是VIO的标配。
SWF可以分成基于滤波器的和基于优化的两种。最典型的基于滤波器的方法就是MSCKF算法了。它是基于EKF的算法，在marginalize state的时候处理比较简单，只需要把对应的covariance的对应行列直接丢弃就可以了。而基于优化的方法在边缘化时需要对Hessian矩阵做舒尔补，操作会复杂一些。
为了尝试一下SWF,我们先从简单的基于滤波的方法入手.本文实现了一个基于MSCKF的Visual+Wheel融合的Odometry.
下面是分别是仿真和用KAIST数据测试的结果
更多可参考
https://zhuanlan.zhihu.com/p/270670373
注意：这里为了凸显加了视觉校正效果，把wheel的noise设的比较大，轨迹不太平滑。如果想要平滑一些，可以适当调低wheel的noise。
代码请见
https://github.com/ydsf16/TinyGrapeKit
传感器配置
Visual部分，用一个单目相机。Wheel部分可以是左右轮速度或位移
坐标系统
轮速坐标系/Odometry坐标系{0}: 车辆后轴中心、贴地. x轴向前, y轴向左,z轴向上.
全局坐标系{G}:与初始时刻的轮速坐标系重合的坐标系。
相机坐标系{C}: x轴向右,y轴向下,z轴向前
内外参数
内参数：1）相机内参；
2）左右轮轴距b，左右轮速系数、将编码器count转成距离米，或者速度转成m/s。
外参数：轮速坐标系到相机坐标系的旋转与平移.
我们假设这些内外参数都假设预先标定好的。实际上，也可以把这些内外参数放到状态向量里面估计，这也是很多论文里面常见的做法，比如OpenVINS。但是，具体这些内外参数能不能估计出来，是不是可观的，可以参考一下黄国权老师的论文
状态向量
滑窗里面的状态分成两类，一类是Odometry的位姿。
另一个类是一串相机位姿：
总的状态是当前Odometry位姿+N帧的相机位姿：
跟MSCKF一样，我们把协方差分块表示：
这里我们使用最简单的滑窗维护方式，当新的一帧进到滑窗后，就直接把老的一帧给边缘化掉。因为是EKF，就是直接把最后一帧相机pose从x中去掉，然后把对应的协方差的行和列删除掉.
Wheel Propagation
EKF算法分成两步：Propagation+Update。在这里，我们用wheel的信息进行状态的propagation，用视觉信息做update。下面先推导Propagation部分。这里的推导，参考了Mingyang Li的paper[2]。
按照IMU处理的方法，首先把ODE方程列出来
下面就可以对ODE方程进行积分了。简单起见，直接用欧拉积分了。如果考虑精度，可以使用中值或者龙格库塔：
用这个式子，就可以进行均值的Propagation。对于协方差的Propgation，我们先求雅克比。这里定义旋转的Error为：
状态增广
当新来一帧图像，可以通过odometry位姿，计算出相机位姿。
然后把它放到状态向量里面。相应的要把协方差矩阵进行扩展：
Update
7.1视觉Update
MSCKF的很大的优势就是没有把特征点放到状态向量里面，降低了计算量。
当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：
-
当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：
-
另一种是边缘化的时候，也就是把最后一帧滑出窗口的时候，把在这一帧里面新建的特征点都丢弃，也就是都拿来做更新。
这里说的特征点/feature，同时表示3D点，也表示在图像上的2D位置。
A. 一个特征点的处理
因为与是正交的.所以
做了上面的操作之后，就可以得到一个不含特征点的线性方程：
B. 多个特征点的处理
上面一个特征点可以得到一个（18）式。多个特征点就可以得到很多的（18）式，把他们堆叠起来，有得到一个很大的线性方程：
这个方程的行数一般很大，直接用来做EKF更新效率很低。MSCKF又用QR分解，进行了一次压缩。具体的对H*做QR分解
带入到(19)式中，可以得到，
左右两边，同时乘以有
最后，我们得到一个压缩后的线性方程
这方程的行数最大和状态的维度相同。最终用来做EKF更新的也就是（20）式。
C. 边缘化
边缘化，或者说如何删除滑窗里的状态。前面也已经提到了，我们使用了最简单的策略。就直接把最老的一帧去掉。去掉的这帧里的所有特征点都被拿来做更新。
最后，总结一下，当一帧图像来了之后的处理步骤
-
增广状态
-
做特征点跟踪。
-
收集跟踪丢失的的所有特征点{}
-
收集将要被边缘化的帧中的所有特征点：{}
-
利用特征点{}和{}构造线性方程(20)，并执行EKF更新。
-
边缘化操作：将x中边缘化掉的pose去掉，将协方差矩阵中对应的行和列删除。
平面约束Update
一般车辆都是运动在平面上的，在更新的时候，我们引入一个平面约束。这部分参考了Stergios I. Roumeliotis 的PaperVINS On wheels[3]
实现
全部工程代码请见：
https://github.com/ydsf16/TinyGrapeKit
实验
仿真测试,很明显,VWO-MSCKF比纯Wheel里程计精度更好.
数据集测试
我们这里使用了KAIST数据集，链接是：
https://irap.kaist.ac.kr/dataset/
同样的,相比纯Wheel odom,精度会有所提高.
注意:这里的测试,Wheel内参数精度都是比较低的,所以raw odom的精度不是很高.如果wheel内参数精度很高的话,VWO-MSCKF精度不一定更高.
本文仅做学术分享，如有侵权，请联系删文。
下载1
在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2
在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。
下载3
在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。
重磅！3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。
一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
觉得有用，麻烦给个赞和在看~
