点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

Plane-Aided Visual-Inertial Odometry for 6-DOF Pose Estimation of a Robotic Navigation Aid

用于机器人导航辅助的6自由度姿态估计的平面辅助视觉惯性里程计

HE ZHANG AND CANG YE (Senior Member, IEEE)

【论文速看】

1、解决了什么问题？

很多论文通过使用视觉里程计的输出来更新IMU的偏置等，但现在还没有很可靠的算法来评估视觉里程计输出的可靠性。

2、提出了什么方法？创新点体现在哪些方面？

（1）提出新的VIO算法，使用3D TOF相机辅助导航；

（2）从当前相机视场的3D点云中提取平面信息，并进行跟踪

（3）提出评估视觉里程计输出精度的算法

3、有哪些需要补充的知识点

（1）3D TOF相机

（2）因子图

【摘要】

经典视觉惯性里程计（VIO）算法通过融合视觉里程计（VO）估计的相机自运动和惯性测量单元（IMU）测量的运动来估计运动相机的6自由度位姿。VIO试图通过在每一步使用VO的输出来更新IMU偏置的估计，从而提高IMU的测量精度。这种方法只有在准确的VO输出能被识别和使用时才有效。然而，没有可靠的方法可以用来进行在线评估的准确性的VO。这篇论文中，介绍了一个新的用于机器人导航辅助（RNA）的位置估计VIO算法，该算法使用了一个3DTOF相机辅助导航。该算法（称作PAVIO）从当前相机视场的3D点云中提取平面，并且通过使用IMU的测量在下一相机视场中跟踪这些平面。算法计算了各跟踪平面参数的协方差矩阵，并且利用参数协方差来进行基于卡方检验的平面一致性检测从而提高VO的输出精度。只有当VO的输出准确时，PAVIO才会接收VO的输出。接收到的VO输出、提取平面的信息以及IMU随时间的测量都被用于构建一个因子图。通过优化图，算法提升了估计IMU偏置的精度并且降低了相机位姿的误差。实验结果验证了该方法的有效性。PAVIO可用于任何基于3D摄像机的视觉惯性导航系统的6自由度姿态估计。

【介绍】

论文提出一种名为“平面辅助VIO”的算法（PAVIO），其通过使用一个3D TOF相机和IMU来进行位姿估计。算法从相机的点云地图中提取平面特征，并且在相机的数据帧上跟踪这些特征，从而在帧之间关联平面特征。接着，使用平面关联信息通过最小化因子图的残差来降低位子估计误差，因子图整合了VO估计的位姿变化、IMU估计的位姿变化以及关联平面的信息。论文的主要贡献：

（1）提出了平面辅助的VIO算法，在图优化框架中，通过将观察到的平面与VO计算的位姿变化估计和IMU的测量紧耦合，提高了位姿估计精度。

（2）提出了一个平面一致性检测算法来检测和剔除VO中不精确的位姿变化。平面一致性检测算法避免提出的VIO算法使用不精确的VO数据，从而获得精度更高的位姿估计。

（3）在一个视障辅助导航的RNA原型的视惯导航系统上实现了所提出的TOF辅助的VIO方法，并在真实环境下进行了实验。

RNA和坐标系

【主要算法】

论文中使用因子图对SLAM问题进行建模。因子图由节点和边组成，节点有两种：变量节点和因子节点。用表示M个关键帧的变量集合，图G由下式定义：

所提出的算法如下：

【实验】

使用RNA收集数据来验证SLAM算法的有效性。在收集数据时，实验者摇动RNA，以正常的行走速度模仿盲人使用手，在每次试验中，实验者都沿着一条环路走，然后回到起点。使用轨迹的终点位置误差范数（EPEN）来作为性能指标。由PAVIO生成的EPPN如下表，同时对比了最新的VIO算法在不同阈值下的表现以及VINS-Fusion的结果。

从Table 可以看出，（1）PAVIO在每个数据及中的表现都优于PAVIO；（2）在位姿估计上，PAVIO的表现要优于VIO算法和VINS-Fusion，其中，PAVIO的平均EPEN为2.47%，VIO算法的平均EPEN为3.75%，VINS-Fusion的平均EPEN为8.1%；（3）使用PCC（平面一致性检测）大大提升了位姿估计精度；（4）PAVIO相比VIO有更好的位姿估计重复性（EPEN的标准偏差为1.03%, VIO的标准偏差为1.74%），意味着在操作环境中更具有鲁棒性。

下图在不同的室内环境中使用RNA，并与VINS-Fusion、VIO等进行比较，展示了使用PAVIO估计姿态为每个实验构建的点云图上这些方法生成的轨迹。可以看出使用PAVIO估计的轨迹精度更高。

【结论】

论文提出了一个新的用于机器人辅助导航位姿估计的视觉惯性里程计算法，其使用了一个3D TOF相机和IMU来进行辅助导航。PAVIO算法从相机当前帧的3D点云中提取平面，并通过IMU测量在下一个相机帧中跟踪平面信息。跟踪的结果用于评估VO的位姿变化估计并且当位姿变化精确时，接受此估计。接受的VO输出、提取的平面信息以及IMU随着时间测测量量，均被用于构建因子图。通过优化因子图，算法提升了系统精度。

【补充知识点】

1、3D TOF 相机

TOF：Time of Flight 飞行时间。所谓飞行时间法3D成像，是通过给目标连续发送光脉冲，然后用传感器接收从物体返回的光，通过探测光脉冲的飞行（往返）时间来得到目标物距离。这与3D激光传感器原理较类似，只不过3D激光传感器是逐点扫描，而TOF相机是通过得到整张图像的深度信息。

TOF相机通过主动发射调制过的光源到目标面上,然后观察计算反射回来的对应光。发射和反射光之间的相位差通过运算和转换得到景深数据。

TOF相机的特点：

• 相对二维图像，可通过距离信息获取物体之间更加丰富的位置关系，即区分前景与后景

• 深度信息依旧可以完成对目标图像的分割、标记、识别、跟踪等传统应用

• 经过进一步深化处理，可以完成三维建模等应用

• 能够快速完成对目标的识别与追踪

• 主要配件成本相对低廉，包括CCD和普通 LED 等，对今后的普及化生产及使用有利

• 借助 CMOS 的特性，可获取大量数据及信息，对复杂物体的姿态判断极为有效

• 无需扫描设备辅助工作

TOF相机缺点：

• 对于普通数码相机，其造价仍然偏高，影响该产品目前的普及使用率

• 相机本身仍然受到硬件发展的限制，更新换代速度较快

• 测量距离较常规测量仪器短，一般不超过 10 米

• 测量结果受被测物性质的影响

• 大多数机器的测量结果受外界环境干扰较为明显，尤其是受外界光源干扰

• 分辨率相对较低，如PMD Camcube 2.0 型号相机，为目前分辨率最高的 3D 相机，其分辨率仅为 204×204像素

2、因子图

因子图是一种表示因式分解的建模工具，属于概率图的一种，概率图有很多种，最常见的就是Bayesian Network(贝叶斯网络)和Markov Random Fields(马尔可夫随机场)。因子图是一种无向图，由两种节点组成：表示优化变量的变量节点，以及表示因子的因子节点。在因子图中，所有顶点，不是变量节点就是函数节点，边线表示他们之间的函数关系。因子图有两种常见的构建方式，一种是学者Forney提出的因子图(下图左)，一种是Loeliger提出的因子图（下图右）

（1）Forney因子图示例                       （2）Loeliger因子图示例

二者各有千秋，Foeney式因子图的优点在于支持分层建模，兼容标准框图，Loeliger式因子图的优点在于方便转换为概率模型。二者都表示了的因式分解，可分解为2个因式分解的乘积：

其中，等式左边的表示全局函数，等式右边的表示局部函数。

从上面两种因子图可以总结出，每个因子图都是由节点和边组成，每个局部函数都可以由唯一的一个因子节点表示，每个变量由唯一的一个变量节点表示，当某一个局部函数与某一个变量相关时，相应的因子节点与变量节点通过边相连。

在SLAM中，可以用因子图表示SLAM的优化模型，SLAM的本质也是一个概率估计问题，也就是对机器人位姿和环境地图特征位置的推算和估计，求解机器人的位姿，即对概率估计问题的求解。对于给定的测量值Z，对未知状态变量X（位姿或地标）的概率估计问题，最常用的估计方法是最大后验概率。引入因子图模型的主要原因是：（1）因子图能够准确区分量测变量和状态变量及其之间的关系；（2）因子图的结构为解决大规模推理问题的计算提供了更为有效的新策略。下图是一个基于SLAM特征图构建的因子图。其中，机器人的位姿被表示为图中的变量节点，观测信息在经过处理后转变为机器人位姿间的约束关系，并通过连接节点间的边来表示。未知状态变量X以变量节点（白色空心圆）表示。

本文仅做学术分享，如有侵权，请联系删文。

下载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天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~