点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
作者：robot L | 来源：知乎
https://zhuanlan.zhihu.com/p/149349921
本文仅做学术分享，如有侵权，请联系删除。
从粗到精的相机自运动估计流程：基于相邻两帧之间的约束进行帧帧位姿估计（相机频率），优化当前帧位姿，得到粗糙的位姿估计；在滑动窗内执行BA，利用窗口内所有约束优化窗口内所有帧的位姿（0.25-1Hz），得到准确的位姿估计。最后融合上述两种估计得到相机频率的准确位姿估计。
分别使用稀疏LiDAR点云/深度图片和三角化获得特征点的深度。
对于有深度和无深度的特征点，构建两种不同约束共同用于位姿优化。
J. Zhang, M. Kaess, S. Singh. Real-time Depth Enhanced Monocular Odometry. IEEE/RSJ International Conference on Intelligent Robots and Systems, 4973-4980, 2014.
J. Zhang, M. Kaess, S. Singh. A real-time method for depth enhanced visual odometry. Autonomous Robots, 41(1), 31-43, 2017.
视觉里程计可以由RGB-D相机/LiDAR提供的深度增强。但是这种深度是有限/稀疏的。有些像素的信息得不到。因此，我们提出一种利用上述稀疏深度恢复相机运动的方法。此外，该方法使用了无深度的特征点，以及通过之前运动三角化得到额外的深度。方法核心是通过BA精修运动。
背景：视觉里程计是给定一系列相机图片来估计自运动。一般，单目是不够的，因为沿着光轴运动会导致视觉特征运动得不充分，从而导致估计问题退化。另外，使用单个相机[1]-[4]，平移不能得到尺度。一般通过假设非完整运动约束[5]，或整合来自IMU的测量来解决该问题。但是，额外测量的质量需要被考虑。此外，通过多相机也能获得尺度。但是，这牺牲了视场大小，因为只有不同相机共同观测的区域被使用。小基线会导致远处的深度不准确；大基线会导致相机标定很困难。因此，本文专注解决相机+稀疏深度的自运动估计问题。
本文方法介绍：与摘要内容相同。
本文方法适用情形：具有（稀疏）深度信息的传感器组合，例如：RGB-D相机；相机+3D LiDAR；双目相机。
图1. 图片帧中跟踪的特征。绿色点表示深度由深度图/LiDAR点云得到；蓝色点表示深度由三角化得到；红色点表示无深度的特征点。
基于视觉的方法现在常用于运动估计[11][12]。
双目：跟踪和匹配图片帧之间的时间特征[13][14]。特征的深度由固定基线的两张图片的三角形几何得到。Paz等人[15]只使用离相机较近（深度较浅）的像素，这有助于恢复平移估计的尺度。Konolige等人[8]使用BA时限双目里程计，同时加入了IMU。
RGB-D相机：Huang等人[20]使用RGB-D相机提供深度的视觉特征计算运动估计。Henry等人[21]整合视觉特征到ICP方法中[22]，通过最小化组合的2D和3D特征匹配误差来实现运动估计。另一种方法是稠密跟踪[23][24]，该方法通过使用环境的稠密3D模型最小化光度误差。但是，上述方法[20]-[24]依赖充足的深度信息。这限制了这些方法应用的环境，比如在深度信息十分缺乏的空旷的环境中可能会失效。
问题：很少RGB-D VO方法能够处理不充足的深度信息。例如Hu等人提供一种启发式转换，能够选择在RGB-D和单目VO之间切换[25]。
我们的方法：我们提供一种处理稀疏深度信息的单目VO方法，通过组合有深度和没有深度的特征。从实验结果来看，该方法比[20]和[23]更鲁棒。
我们使用右上标 来表示相机帧。定义相机系统 是一个3D坐标系统，原点在相机光心。x轴指向左方，y轴指向上方，z轴指向前方并作为相机的主轴。
我们使用有和无深度的特征。令 为一组特征点。对于每个特征 ，若有深度，则在 中的特征被记为 ；若无深度，则坐标被归一化使得z坐标等于1，记作 。有了上述记号，我们的视觉VO问题可以被定义如下：
问题：给定一系列图片帧 和特征 ，计算相机在两个连续帧 和 之间的运动，如果特征已知深度，则已知使用 ，否则使用深度 ， 。
提出的方法在三种不同的传感器系统中进行了验证。前两种传感器如图2所示；第三种使用KITTI基准数据集的配置。
图2. 评估的传感器配置。(a) RGB-D相机 (b)相机和3D LiDAR组合。
图3展示了软件系统的示意图。首先，视觉特征在Feature Tracking模块被跟踪。然后在Depth Map Registration模块，深度图片/点云和RGB图片对齐，并且关联深度和视觉特征。接着，具有深度/没有深度的特征被传到Frame to Frame Motion Estimation模块，输出粗糙高频位姿（相机频率）。特征和位姿传送到BA模块，进一步精修位姿，输出精确低频位姿（0.25-1Hz）。最后整合两种位姿，得到精确高频位姿。
图3. 视觉VO的框图
我们从帧帧运动估计的推导开始，对应算法在下一节讨论。记得 和 为在 和 跟踪的特征。定义 和 为两帧间的旋转矩阵和平移向量，我们可以建模相机运动如下：
(1)
接着，我们讨论具有深度的特征。在 帧时刻， 帧的深度图已经可用，并且 的深度已经被关联，具体深度关联过程见5.C节。记得 是 的归一化项，我们重写(1)为
(2)
等式(2)包括三行，消去 后得到
(3)(4)
其中 和 是 和 的第 行。
对于未知深度的特征，我们重写(1)如下：
(5)
消去 和 后，我们得到
(6)
假设我们公有 个已知深度的特征和 个未知深度的特征。组合(3)(4)和(5)我们共同得到 行等式：
(8)
其中 是 残差向量，并且 是待优化的平移和旋转分量。 相对 的雅克比记作 。(8)可以被解决通过LM算法 [26]，
(9)
其中， 是LM方法中的一个尺度因子。
帧帧运动估计的伪代码如算法1所示。
算法1. 帧帧运动估计伪代码。
本节的目的是得到k-1到k帧被跟踪的特征在k-1帧中的深度。首先将维护的深度图投影到k-1帧的单位球坐标系中（距离归一化球面，球上所有点到k-1帧相机光心都是单位距离），然后均匀采样深度图。对于k-1帧中每个被跟踪的特征点，在采样后的深度图中寻找离该特征点距离最近的三个点。如果三个对应点在3D空间中距离足够近，则根据该三个深度点插值得到该特征点的深度。过程详见robot L：【论文阅读13】Laser-visual-inertial odometry and mapping的5.3节。
上一节的相机帧帧运动估计被集束调整精修，其中从前40相机帧中取出8帧进行精修，这样做的目的是保证再下次选择新的帧进行精修之前上次集束调整已经结束。
这里，我们定义特征的另一种表示 ，其中x和y坐标是归一化坐标，z坐标表示深度。对于没有深度的特征， 被设置为缺省值。令 表示待优化的图片帧集合， 是集合中的首帧。初始化后，所有特征被投影到 ，被记作 。定义 为从 到 的变换，其中 是集合中的其他帧， 。BA最小化下述函数：
(11)
其中 表示特征 在帧 中的观测， 是信息矩阵，前两项为给定的常数值，第三项对于具有深度的像素取较大值，三角化得到深度的项像素取较小值，无深度的像素取零值。
该方法使用KLT方法[31]跟踪Harris角点[26]。特征跟踪和BA各占一个核，帧帧运动估计和深度配准共同占一个核。
实验环境如图6所示共4种：会议室，大堂，杂乱的道路和平坦的草坪。难度依次递增，因为环境越来越开阔，深度信息越来越稀疏。图6的第2行和第4行展示了4个场景某帧的深度收敛图，左图是RGB-D相机的收敛结果；右图是LiDAR的收敛结果。对于RGB-D相机来说，四个场景的像素深度收敛率从94%降低到23%。对于LiDAR来说，四个场景的深度收敛率从89%降低到47%。
图6. 作者收集的数据集的轨迹对比和深度收敛图。
与两个流行的RGB-D视觉里程计方法进行了比较。Fovis估计了相机的运动通过跟踪相机特征，特征的深度从深度图片中获得[20]。DVO是一个稠密跟踪方法，最小化整张图片的光度误差[23]。每个场景的起点和终点相同，使用估计的起点和终点位置差与整条轨迹的长度比值作为相对位置差异，定量结果如表1所示。
表1. 不同方法在不同场景中的相对位置差异。
从结果来看，我们的方法在四个场景中的表现都不错。在深度信息较多的场景（Room），其余两种对比方法表现良好，但是在其他深度信息不足的场景中，Fovis经常不生成位姿估计由于内点特征不够；而DVO一直产生位姿估计，但是漂移很大。
图7展示了三个环境的结果。第一行中，提出的使用BA和不使用BA的结果和真值轨迹进行对比。在中间一行，图片和对应激光点云被展示，激光点使用深度染色。完整的11个序列的结果如表2所示。
表2. KITTI测试集的设置和结果。
进一步地，为了探究BA的影响，我们比较了在三种环境（市区，高速，乡村）中的准确度，结果如图8所示。
图8. 在三种场景的相对位值误差。黑色，蓝色和红色分别表示100%，75%和中值。
本文仅做学术分享，如有侵权，请联系删文。
推荐阅读：
专辑|相机标定
专辑|3D点云
专辑|SLAM
专辑|深度学习与自动驾驶
专辑|结构光
专辑|事件相机
专辑|OpenCV学习
专辑|学习资源汇总
专辑|招聘与项目对接
专辑|读书笔记
重磅！3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近1000+星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
3Ｄ视觉工坊 认证博客专家 算法 3D视觉 个人公众号：３D视觉工坊。公众号特邀嘉宾及合伙人，先后就职于国内知名研究机构、自动驾驶公司、海康研究院，主要研究方向为深度学习、目标检测、语义分割、图像处理、自动驾驶感知算法等，博客专家。博主先后任职于国内知名研究院、知名大厂，致力于3D视觉算法、VLAM算法开发，涉及相机标定、手眼标定、结构光、点云后处理、三维重建等相关领域的研究，同时也是博客专家。3D视觉工坊坚持原创，近一年来输出了非常多的高质量文章，获得了粉丝的一致好评，我们将始终坚持走原创路线，打造一个铁杆粉丝的聚集区。