欢迎大家再次回到泡泡机器人的课堂。不久前，ORB-SLAM2的作者在原纯视觉SLAM的基础上，针对ORB-SLAM的一些固有问题，提出了一种全新的结合IMU的方法[2]。就在几天前，王京大神开源了基于该算法的一种实现[1]。本文中，王京大神将为大家详细讲解该实现中的关键技术，希望本文可以帮助大家刚好地阅读和理解相关代码。
1.前言这份学习代码[1]参考VisualInertialORB-SLAM的论文[2]，从ORB-SLAM2[3,4]的基本代码修改而来。目前开源的VisualInertial系统有OKVIS和ROVIO，但用到IMUPreintegration[5,14,15]的暂时没开源，只在GTSAM[6]中包含了相关的IMUfactor，而ORBSLAM作者的这部分代码暂时也没有开源。自己在尝试上面论文的方法之后感觉论文里初始化挺实用的、整体程序也不很难，所以就在写完之后放到了github上供大家一起学习交流。由于能力有限，如果有bug还请包含并指出，作者尽力修正，也欢迎大家一起玩、一起改进。本文档给出简单的说明，以及主要的参考资料。2.ROS入口这份代码暂时只写了rosbag的接口，入口在Examples/ROS/ORB_VIO/ros_vio.cc中。每次读取一个图像msg和到这个图像的一组IMUmsg，在Tracking和LocalMapping计算完之后再对下一个图像msg和下一组IMUmsg进行处理（所以并不是实时的）。（注：冯余剑同学提供了去掉ROS的版本，在github的issue里放了链接。）3.IMU参数目前只在EuRoc数据集[7,8]上进行了测试，IMU的参数写死在IMU/imudata.cpp中，噪声参数来源于数据集中的sensor.yaml，计算参考Kalibr[9,10]的wiki中所述IMU噪声模型[11]。如这个wiki中所说，datasheet上所给的理论值一般是理想情况下的，实际使用时看情况要放大一些，所以程序中计算时有进行放大（但也只是大致试了几个值，选了一个看起来好点的）。另外，EuRoc的数据中IMU和相机之间没有延时，自己的数据需要Kalibr进行标定。（注：这两天在自己的硬件数据上跑起来了，见下文说明）4.IMU预积分（Preintegration）（Preintegration的实现在IMU/IMUPreintegrator.h和IMU/IMUPreintegrator.cpp中）VisualInertialORBSLAM中，IMU的使用方式是把相邻帧或相邻关键帧之间的IMU测量值利用预积分的方式计算一个近似高斯分布的“伪”测量量，这种方式可以把IMU的噪声参数传递给这个伪测量量，得到预积分的不确定度。利用这个预积分作为帧间的观测量加入优化中。此外，预积分能给出伪测量量相对于零偏变化的雅克比，这样在零偏有修正时，可以计算一阶近似值，而不用重复计算积分，减小计算量。预积分的公式参考Forster的论文和补充材料[5,14,15]，需要说明的是早先的两篇[14,15]中公式有稍许错误（3/2和一些下标不对）。SO3上左右雅克比、Adjoint等相关的理论公式可以参考[16]的7.1节，或者联系高博的新书《视觉SLAM十四讲》。谢晓佳也有一份预积分的“部分”总结文档，可以在qq群文件中找到。但感觉最好都自己手推一遍。5.待估计状态NavState（NavState的实现在IMU/NavState.h/和IMU/NavState.cpp中）NavState名字来源于GTSAM，包含位置P、速度V、旋转R、陀螺零偏bg、加速度计零偏ba。除此以外，考虑到优化过程中要计算零偏的变化量，但预积分的方式保持了零偏不变，所以在NavState中加入了陀螺和加速度计的零偏增量dbg/dba。每当新建一个关键帧时，会利用到目前为止已经估计好的当前帧零偏bg/ba及零偏增量dbg/dba来初始化关键帧的零偏为bg+dbg/ba+dba，而把零偏增量初始化为0。在优化更新过程中，只更新增量dbg/dba而保持bg/ba不变（这样就不用重新计算预积分了）。NavState的更新方式参考了论文[14]的补充材料[15]，应该也可以有其他方式。6.VINS初始化（这部分在LocalMapping.cc的TryInitVIO()函数中）对单目VI系统来说，个人感觉初始化是比较麻烦的部分。之前很多的工作可能是用IMU初始化一个初始姿态，然后慢慢估计和收敛尺度，或者由其他传感器提供尺度，这些相关工作了解还不是特别多，但感觉不那么“优雅”。感觉比较优雅的方式：沈劭劼老师有专门针对单目VINS无先验要求的初始化的讨论[12,13]，但暂时没有开源，自己试着写了个挫挫的视觉前端跑起来并不好用，还是需要个好的视觉前端的……在此基础上，ORBSLAM的作者对[13]的做法进行了改进，利用已有的纯VSLAM的地图，分步骤来估计陀螺零偏、加速度计零偏和尺度、重力加速度。按照论文的介绍实现之后，发现可以基本复现论文中两个数据集初始化的结果（从这个角度看感觉这方法还比较实用）。具体的方法参考论文，写的程序中以学习为目的，所以在前15秒，每来一个新的关键帧，都会用所有的关键帧进行一次计算，把结果存下来，然后可以用pyplotscripts中的文件画出初始化各变量的变化情况。需要说明的是，我推导公式的时候，发现和论文中部分符号不太一致，建议自己整理推导下。程序中重力方向认为是(0,0,1)，这应该并不影响最终结果。7.新加g2o相关节点/边（相关实现在IMU/g2otypes.h和IMU/g2otypes.cpp中）因为在ORBSLAM的框架下，优化用的库是g2o，需要根据需要增加g2o的edge/vertex。具体的对照论文中的图（原文中Fig2）。VertexNavStatePVR中包含了上图中的位姿P和速度v，VertexNavStateBias中包含了上图中的IMU零偏b。（按作者图里的意思，他可能把PR和V也分开成两类vertex了。）EdgeNavStatePVR作为上图中上面的绿框所指edge，是一个多元边，链接相邻两帧的PVR和两帧中较早一帧的Bias。它的测量（测量值和不确定度）由预积分给出。EdgeNavStateBias作为上图中下面的绿框所指edge，连接相邻两帧的bias。它的测量的值保持为0，不确定度由IMU随机游走参数确定。EdgeNavStatePVRPointXYZ作为上图中左边蓝框所指edge，连接PVR和地图点。EdgeNavStatePVRPointXYZOnlyPose作为上图中右边蓝框所指edge，是一个一元边，用于poseOptimization中的poseonlyBundleAdjustment。EdgeNavStatePriorPVRBias作为上图中的灰色框所指edge，包含marginalize之后的当前帧prior信息。（这里需要说明，计算marginalize的结果用到g2o的白癜风复发扩散白癜风临床专家