CodeVIO:基于可学习优化密集深度的视觉惯性里程计（ICRA2021）

(Xingxing Zuo, Nate Merrill, Wei Li, Yong Liu, Marc Pollefeys, and GuoquanHuang.CodeVIO: Visual-Inertial Odometry with Learned Optimizable DenseDepth.IEEE International Conference on Robotics and Automation (ICRA) 2021.)

摘要

论文提出了一个轻量级的紧耦合深度估计网络和视觉惯导里程计系统，可以提供准确的状态估计和实时的稠密深度图估计，利用轻量级的条件变分自动编码器（CVAE）对深度进行编码，通过给网络提供已经边缘化的VIO稀疏特征来增加初始深度估计的准确性和网络的泛化能力，之后利用估计得到的稠密深度联合VIO滑动窗口来恢复局部的场景几何信息。论文还提出了一种新颖的方法来获取CVAE的雅可比矩阵，该方法的计算速度对比之前的工作有一个数量级的提升，此外论文还利用了FEJ避免重新计算。与之前完全依赖稠密残差不同的是，论文只提供稀疏观测来更新深度，实验表明稀疏观测配合FEJ仍可以显著改善估计的深度图，该系统可以实时单线程运行，仅需要GPU对网络和编码Jacobian进行加速。

论文贡献

1. 提出了一种稀疏特征深度估计的VIO系统，可以实时单线程运行，只需要GPU对网络和Jacobian进行加速。

2. 提出了一种轻量级的CVAE模型，该模型利用深度可分离的卷积和加法跳跃连接以实现高效预测，而保持较高的准确性。通过CVAE，稠密的深度图被编码为低维深度编码，可以与VIO状态向量一起更新。

3. 提出了一种新的方法恢复VAE解码器Jacobian，计算速度相对于Pytorch链式法则有了一个数量及的提升，并利用FEJ避免了重复计算。

4. 通过融合IMU信息，将最近边缘化掉的图像帧稀疏特征点加入网络中以提高初始深度的和零编码先验的准确性，极大地提高了系统精度。

算法流程/算法概述

论文设计了一种CVAE神经网络，通过将RGB图像或者灰度图作为网络的输入，级联VIO稀疏深度图，来预测稠密的深度图及其方差，并利用变分自动编码器（VAE）对预测得到的稠密深度图进行编码，通过滑动窗口策略同VIO状态向量一起紧耦合联合估计。论文利用MSCKF框架对状态向量进行更新，采用了固定时间间隔的关键帧策略，分别对重投影误差和深度几何误差做出了推导，针对深度几何约束提出了一种新的扰动Jacobian计算方法，并联合FEJ有效地降低了计算复杂度。

1、快速深度预测和编码

▴深度预测和编码网络

论文受到CodeSLAM的启发，将深度预测网络分为两个部分：

1) 一个修剪过的FASTDepth网络，输入为GRB图像或者灰度图，级联稀疏深度图，来预测稠密的深度图及其方差。

2)一个变分自动编码器，对第一部分的深度信息进行编码，生成一个可以放入VIO状态向量中联合优化的深度向量（Depth Code），并在优化完之后可以解码出稠密恢复深度图。

在训练和验证阶段，为了模拟VIO稀疏特征点，论文在groundtruth的稠密深度图中，提取FAST角点，并随机采样50到200个关键点，为了缓解噪声的影响，论文注入了标准差为0.1m的高斯噪声来采样稀疏特征点，避免过拟合理想均匀分布。为了保证低时延，所有的卷积层都是深度分离的，在特征提取和自动编码阶段都采用高效的MobileNet。为了降低计算复杂度，所有的跳跃连接都是采用的加法而不是像CodeSLAM那样的级联策略，跳跃连接提供了低层次的梯度和尺度信息，为了充分保留这些信息，论文额外增加了元素对应相乘项来压缩加法跳跃连接的信息，论文发现加法跳跃连接极大地加速了网络的计算，而只造成极小的精度损失。

论文训练的loss是深度恢复loss和 VAE KL发散loss的总和，VAE KL发散loss最小化深度编码分布和标准正态分布之间的差异，深度恢复loss具体形式如下：

x表示每一个像素点，D表示的是像素点的逆深度，B表示对应深度的不确定度。

2、整个系统待优化的状态向量表示

本文利用OpenVINS，通过扩展MSCKF，实现单目VIO和局部稠密建图，整个系统的转态向量包括IMU状态量，历史相机位姿，相机和IMU外参数，相机内参数，IMU和相机时间同步，SLAM 特征点，以及对应点的深度编码（depth code，32维的向量），具体参数如下：

m表示历史所有的相机帧数，p表示窗口的大小，n表示历史已有的关键帧个数，c表示已有的关键帧深度编码，A表示局部坐标系（通常采用首次观测到某一个三维点或者跟踪到的该点的最后一帧图像作为该点的局部坐标系）。论文采用固定时间间隔的关键帧策略，每隔n帧就取一个关键帧，避免了深度图的冗余计算，同时也保证了在每一个滑动窗口中都有一个深度编码信息。

3、重投影误差和稀疏几何更新

论文利用MSCKF进行状态估计和稠密局部建图，通过FEJ提高了一致性同时也避免了计算深度编码的Jacobian。IMU 预积分项采用和MSCKF相同的计算方法，利用前一帧来预测当前帧的状态，并对IMU状态和相机位姿进行增广，如果当前帧是关键帧，还需要对深度编码进行增广。深度编码初始化为零向量，方差为单位矩阵数乘不确定度，这和深度编码服从标准正态分布是一致的，在实际使用中，由于存在偏差，论文采用的标准差大于1。

论文的关键思路是如何利用深度编码来构建视觉残差项，由于关键帧的深度值受到原始图像M、稀疏深度图S、以及深度编码的估计值的约束，可以写成如下关系形式，f是一个非线性的函数。

依据MSCKF逆深度和三维点局部参考帧坐标系的关系，滑动窗口内的重投影误差可以写成如下形式：

H表示对状态向量和三维点坐标的雅克比矩阵，n表示服从高斯分布的白噪声，A表示参考坐标系，p表示三维点的在参考帧中的坐标。由于三维点的坐标会受到参考帧图像中的二维观测以及关键帧稠密深度图中对应的深度的约束，也就是会受到深度编码的约束，此时可以构建空间几何约束如下：

e3的转置乘上三维点的坐标表示取得三维点的z值，也就是对应的深度值。

也就是说，从参考帧中投影到当前帧中的深度需要与深度编码所对应的深度保持一致性。G表示对深度编码、IMU状态向量以及三维点坐标的雅克比矩阵。

n表示深度编码白噪声，服从高斯分布，可以直接从深度预测网络中获得到深度编码的不确定度，需要注意的是，对于不同的关键帧，不确定度的大小是不一样的。

由于稠密深度图的计算和原始图像、稀疏深度图以及深度编码有关，而稀疏深度图中的深度信息通常会随着MSCKF 特征点的边缘化被整合，从而使得D不再与特征点的三维坐标相关，因此论文把原始图像和稀疏深度图当作是先验信息，作为恒定约束，来提高深度零编码的精度，对于EKF和FEJ系统来说，一个更准确的初始值，将会极大地提高系统的精度。

4、 深度一致性更新

同一个被跟踪到的三维点在不同的关键帧中的深度应该是一致的，将关键帧Ca中的三维点投影到关键帧Cb中，与Cb深度编码所对应的深度应该是相等的。

oi和oj表示在关键帧Ca和关键帧Cb所对应的图像二维观测坐标，ca和cb分别表示在关键帧Ca和关键帧Cb中的深度编码，xab表关键帧Ca和关键帧Cb之间的相对位姿变换，D表示深度编码对应的深度，W表示对应的雅克比矩阵，n表示高斯白噪声。

到目前为止，论文中所有的观测残差项已构建完毕，之后按OpenVins中的更新策略来边缘化MSCKF三维点，更新状态向量，从而求解位姿。

5、高效的网络雅克比计算

论文中特意指出，在进行更新的时候，需要求解深度值对深度编码的雅克比矩阵，由于这是一个非线性的网络，通常深度学习优化库会存储梯度向量，而不是完整的雅可比矩阵，这一项雅克比的计算在PC上通常很难实时计算，Czarnowski 等人在GTX 1080Ti GPU上使用TensorFlow计算该项雅克比矩阵超过300ms，这无法保证实时性。

由于只需要计算深度值对深度编码的雅可比矩阵，而并不需要所有的中间结果，论文提出不需要完整地按链式法则进行计算，而是采用如下增量式计算方法：

论文指出深度解码器是一个神经网络，通常是批量处理数据，而深度编码论文中采用32*1维的向量，因此只需要针对几个有代表性的极小增量值，批量跑一个最小patch即可，在GTX 1080Ti GPU上大约10ms，在GTX 1060Ti GPU上大约40ms，比之前的工作快了一个数量级。

­主要结果/结果呈现

本论文的深度预测精度如下表所示：

论文在GTX 1080Ti GPU 、Intel i7-8086k CPU@4.0GHz.的电脑上，在 NYU Depth V2 data数据集上，分别对精度和速度进行评测，Sp表示带有稀疏深度图输入，整个系统既可以运行在带有稀疏深度图输入的模式下，也可以不输入稀疏深度图，可以看到带稀疏深度图的效果最好，而灰度图和彩色图差异不大。

稠密深度图效果图如下图所示，左边第一列表示RGB图像，第二列表示深度gt，第三列表示预测深度图，第四列表示预测深度图RMSE，第五列表示稀疏深度图输入，第六列表示带有稀疏深度图输入的预测深度图，第七列表示带有稀疏深度图输入的预测深度不确定度，第八列表示带有稀疏深度图输入的预测深度图RMSE。可以明显看到，稀疏深度图输入有效地减小了深度误差，深度不确定度也很好地贴合预测深度的误差。

本论文的网络雅克比矩阵精度如下图所示，在不同的增量值大小下，雅克比矩阵的精度会有所变化，可以看到当增量取值在0.5到1.0之间时误差较小，在本论文中，增量值取0.666。

系统整体的精度评估，分为对比深度值为VIO稀疏深度图、深度值为网络深度预测结果，以及深度值为网络深度预测结果加上重投影和几何约束更新后的深度结果。可以看到，VIO稀疏深度图虽然有噪声，但是精度仍然最高，单纯的网络深度预测精度稍差一点，而更新之后的结果也要优于零编码，同时也表明了建图和位姿估计是相互促进的，定位精度更高，优化出来的深度图也会更准确。

通过对比OpenVINS 和CodeVIO ，即包含稀疏深度图输入和稀疏几何更新的完整系统，可以看到，论文所提出的方法在精度上和OpenVINS差不同，在平均精度上稍好于OpenVINS，单帧处理时间约为0.0447 ± 0.0226 秒，可以满足大部分应用的实时性要求。

笔者点评

该论文设计了一种带有实时稠密重建的VIO系统，提出了一种新颖的CVAE神经网络用来估计深度并对深度进行编码，充分利用了VIO稀疏深度图的信息，并且拥有很强的泛化能力。稠密的深度信息被整合为低维的编码，可以同传统VIO系统的状态向量一起紧耦合优化，在轻量级的MSCKF上，该论文的精度达到了start-of-art的结果。此外该论文提出了一种高效的网络Jacobian计算方法，相比于传统的链式法则，速度提高了一个数量级。

该论文的精度还高度依赖于稀疏深度图输入，整个系统基本在OpenVINS的框架下，精度相比于OpenVINS的结果并没有较大的提升。另外在不用稀疏深度图输入的模式下，论文并没有更详细地对比和传统VIO或者SLAM算法的精度差异，尤其在传统VIO不能成功运行的数据集中，如弱纹理、大场景、快速旋转等场景下，单纯地依赖神经网络的深度图和不确定度估计能否获得较高的精度结果并没有做进一步的对比。

参考文献

[1] M. Bloesch, J. Czarnowski,R. Clark, S. Leutenegger, and A. J. Davison.“CodeSLAM—learninga compact, optimisable representation for dense visual SLAM”. In:Proceedings of the IEEE conference on computer vision and pattern recognition.2018, pp. 2560–2568.

[2] J. Czarnowski, T. Laidlow,R. Clark, and A. J. Davison. “Deepfactors: Realtime probabilistic dense monocularslam”. In: IEEE Robotics and Automation Letters 5.2 (2020), pp. 721–728.

[3] Z. Yang, F. Gao, and S.Shen. “Real-time monocular dense mapping on aerial robots using visual-inertialfusion”. In: 2017 IEEE International Conference on Robotics and Automation(ICRA). IEEE. 2017, pp. 4552–4559.

[4] A. Rosinol, M. Abate, Y.Chang, and L. Carlone. “Kimera: an open-source library for real-timemetric-semantic localization and mapping”. In: 2020 IEEE InternationalConference on Robotics and Automation (ICRA). IEEE. 2020, pp. 1689–1696.

[5]K. Lasinger, R. Ranftl, K.Schindler, and V. Koltun. “Towards robust monocular depth estimation: Mixingdatasets for zero-shot cross-dataset transfer”.In: arXiv preprintarXiv:1907.01341 (2019).

[6]D. Wofk, F. Ma, T.-J. Yang,S. Karaman, and V. Sze. “Fastdepth: Fast monocular depth estimation on embeddedsystems”. In: 2019 International Conference on Robotics and Automation (ICRA).IEEE. 2019, pp. 6101–6108.

[7]F. Ma and S. Karaman.“Sparse-to-dense: Depth prediction from sparse depth samples and a singleimage”. In: 2018 IEEE International Conference on Robotics and Automation(ICRA). IEEE. 2018, pp. 1–8.

[8]J. Tang, F.-P. Tian, W. Feng, J. Li, and P. Tan. “Learning guidedconvolutional network for depth completion”. In: arXiv preprintarXiv:1908.01238 (2019).

[9]K. Tateno, F. Tombari, I. Laina, and N. Navab. “Cnn-slam: Real-timedense monocular slam with learned depth prediction”. In: Proceedings of theIEEE Conference on Computer Vision and Pattern Recognition. 2017, pp. 6243–6252.

[10]N. Yang, R. Wang, J. Stuckler, and D. Cremers. “Deep virtual stereoodometry: Leveraging deep depth prediction for monocular direct sparseodometry”. In: Proceedings of the European Conference on Computer Vision(ECCV). 2018, pp. 817–833.

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