VINS-MONO翻译以及理论推导系列(四)：VIII全局位姿图优化、IX实验结果

                                           VIII. 全局位姿图优化

       重新定位后，局部滑动窗口移动并与过去的姿态对齐。利用重定位结果，提出此位姿图优化步骤以确保将过去姿势的集合登记为全局一致配置。

       由于我们的视觉惯性设置使滚动和俯仰角完全可观测，累积漂移只发生在四个自由度(x，y，z和偏航角)。为此，我们忽略了对无漂移的滚转和俯仰状态估计，只进行了四自由度位姿图的优化。

A. 位姿图中添加关键帧

当关键帧从滑动窗口中边缘化时，它将被添加到位姿图中。这个关键帧在位姿图中充当顶点，它通过两种类型的边与其他顶点连接：

1）序列边：关键帧将建立与其先前关键帧的若干顺序边。序列边缘表示局部滑动窗口中两个关键帧之间的相对变换，其值直接从VIO中获取。考虑到新边缘化的关键帧i及其先前的一个关键帧j，序列边缘只包含相对位置和偏航角 。

2）闭环边：如果新边缘化的关键帧有一个循环连接，在姿态图中它将通过一个闭环边与闭环帧连接。同样，闭环边只包含与(27)相同定义的四自由度相对位姿变换。利用重定位的结果，得到了闭环边的值。

B.4自由度位姿图优化

我们将帧i和j之间的边的残差定义为：

其中，是直接从单目VIO中得到的滚转角和俯仰角的估计。通过最小化以下代价函数，对顺序边和回环边的整个图进行了优化：

其中S是所有序列边的集合，L是闭环边的集合。尽管紧耦合的重定位已经有助于消除错误的闭环，但我们添加了另一个Huber范数(·)，以进一步减少任何可能的错误循环的影响。相反，我们不对序列边使用任何鲁棒范数，因为这些边缘是从VIO中提取出来的，VIO已经包含了足够多的外点排除机制。

       位姿图优化和重定位异步运行在两个独立的线程中。以便在可用重定位时，能立即使用最优化的位姿图。同样，即使当前的姿态图优化尚未完成，仍然可以使用现有的姿态图配置进行重新定位。这一过程如图9(b)所示。

C. 位姿图管理

随着行程距离的增加，姿态图的大小可能会无限增长，从而限制了系统的实时性。为此，我们实现了一个下采样过程，以将位姿图数据库保持在有限的大小。所有具有闭环约束的关键帧都将被保留，而其他与其邻居方向过近或方向非常相似的关键帧可能会被删除。关键帧被移除的概率和其与邻居空间密度成正比。

                                        IX. 实验结果

我们进行了三个实验和两个应用，以评估所提出的VINS-Mono系统.在第一个实验中，我们将所提出的算法与另一种最新的基于公共数据集算法进行了比较.通过数值分析，验证了系统的精度。然后在室内环境中测试我们的系统，以评估在重复场景中的性能。通过大量的实验验证了系统的长期实用性.此外，我们还将所提出的系统应用于两个应用程序。对于空中机器人的应用，我们使用vins-Mono作为位置反馈来控制无人机跟踪预定的轨迹。然后我们将我们的方法移植到iOS移动设备上，并与GoogleTango进行比较。

A. 数据集比较

我们使用EuRoc的MAV视觉-惯性数据集评估我们提出的VINS-Mono。这些数据集是在一架微型飞行器上收集的，它包含立体图像(Aptina MT9V034全球快门、WVGA单色、20 FPS)、同步IMU测量(ADIS 16448、200 Hz)和地面真实状态(Vicon和Leica MS 50)。我们只使用左边相机的图像。在这些数据集中观察到了较大的IMU偏置和光照变化。

在这些实验中，我们将vins-mono和OKVIS进行了比较，这是一种最先进的VIO，可以用单目和立体相机工作。OKVIS是另一种基于优化的滑动窗口算法.我们的算法与OKVIS在许多细节上是不同的，如技术部分所示。我们的系统具有鲁棒的初始化和闭环功能。我们使用MH-03和MH-05两种序列来证明该方法的性能。为了简化表示法，我们使用vins来表示我们只使用单目VIO的方法，而vins_loop表示含重定位和位姿图优化的完全版本。我们分别用OKVIS_Mono和OKVIS_stereo分别表示OKVIS使用单目和立体相机的结果。为了进行公平的比较，我们丢弃前100个输出，并使用接下来的150个输出对齐地面真值，并比较其余的估计器输出。

MH-03序列轨迹如图11所示。我们只比较平移误差，因为旋转运动在这个序列中是可以忽略的。图12显示了x,y,z误差与时间的关系，以及平移误差与距离的关系。在误差图中，具有回环的vins-mono具有最小的平移误差.我们在MH 05上观察到类似的结果。该方法具有最小的平移误差。平移和旋转误差如图14所示。由于该序列运动平稳，偏角变化不大，只发生位置漂移。显然，闭环可有效地约束累积漂移。OKVIS在滚动和俯仰角度估计方面表现更好。一个可能的原因是VINSMono采用了预积分技术，即IMU传播的一阶近似，以节省计算资源。

       vins-Mono在所有Euroc数据集中表现良好，即使在最具挑战性的序列中，V1-03包括剧烈性的运动、纹理减少的区域和显著的光照变化。由于采用了专用的初始化过程，该方法在V1-03中可以快速初始化。

       对于纯VIO，vin-Mono和OKVIS具有相似的精度，很难区分哪个比较好。然而，vins-Mono在系统级别上优于OKVIS。它是一个完整的系统，具有鲁棒的初始化和闭环功能来辅助单目视觉。

B. 室内试验

在室内实验中，我们选择实验室环境作为实验区域。我们使用的传感器套件如图15所示。它在DJIA3控制器中包含一个单目照相机(20Hz)和一个IMU(100 Hz)。我们手握传感器套件，在实验室以正常的速度行走。如图16所示，我们遇到行人，光线较弱，纹理较少，玻璃和反射。视频可以在多媒体附件中找到。

我们将我们的结果与OKVIS进行了比较，如图17所示。图17(a)是OKVIS的VIO输出。图17(b)是所提出的无回环方法的VIO结果。图17(c)是所提出的具有重定位和闭环方法的结果。当我们在室内循环时，会出现明显的漂移。OKVIS和只有VIO版本的vins-Mono在x,y,z和偏航角上积累了大量漂移.我们的重新定位和循环关闭模块有效地消除了所有这些漂移。

C. 大尺度场景

       1)走出实验室：我们在室内和室外混合的环境中测试vins-mono。传感器套件与图15所示的相同。我们从实验室的一个座位上开始，在室内空间里走来走去。然后我们下了楼梯，在大楼外的操场上走来走去。接下来，我们回到楼里上楼。最后，我们回到了实验室的同一个座位。整个轨道超过700米，持续约10分钟。在多媒体附件中可以找到实验的视频。

       轨迹如图19所示。图19(a)是OKVIS的轨迹。当我们上楼时，OKVIS显示出不稳定的特征跟踪，导致估计错误。我们看不到红色街区楼梯的形状。VINS-Mono的唯一结果如图19(b)所示.有闭环的轨迹如图19(c)所示。该方法的楼梯形状清晰。闭环轨迹与谷歌地图对齐，以验证其准确性，如图18所示。

       OKVIS x,y和z轴的最终漂移为[13.80,-5.26,7.23]米。VINS-Mono无闭环的最终方向为[-5.47,2.76,-0.29]m，占总轨迹长度的0.88%，小于OKVIS的2.36%。经回环修正，最终漂移有界于[-0.032,0.09,-0.07]m，与总轨迹长度相比，这是微不足道的。虽然我们没有地面真值，但我们仍然可以直观地检查优化后的轨道是否平滑，并能精确地与卫星地图对齐。

2)环游校园：这张环绕整个科大校园的非常大规模的数据集是用一个手持的VI-Sensor 4记录下来的。该数据集覆盖的地面长度约为710米，宽度为240米，高度变化为60米。总路径长度为5.62km。数据包含25赫兹图像和200赫兹IMU，持续1小时34分钟。对VINS-Mono的稳定性和耐久性进行测试是一个非常有意义的实验。

在这个大规模的测试中，我们将关键帧数据库的大小设置为2000，以提供足够的循环信息并实现实时性能。我们在3.60GHz的英特尔i7-4790 CPU上运行此数据集。表1显示时间统计数据。估计的轨迹与图20中的谷歌地图一致。与谷歌地图相比，我们的结果在这个非常长时间的测试中几乎没有漂移。

D. 应用1: 空中机器人反馈控制

如图21(a)所示，我们将vins-Mono应用于航空机器人的自主反馈控制。我们使用了一个具有752×480分辨率的前视全球快门相机(MatrixVisionMvBlueFOXMLC200w)，并配备了190度鱼眼镜头。DJIA3飞行控制器用于IMU测量和姿态稳定控制。机载计算资源是3.00GHz 的Intel i7-5500 U CPU。传统的针孔摄像机模型不适用于大视场摄像机。我们使用MEI模型对此相机进行校准，由[43]中引入的工具包进行校准。

在本实验中，我们使用VINS_Mono的状态估计来测试自主轨迹跟踪的性能。在这个实验中，回环检测被禁止。使四旋翼跟踪一个数字8模式，每个圆圈半径为1.0米，如图21(b)所示。在轨迹周围设置了四个障碍物，以验证VINS-Mono无闭环的准确性。在实验过程中，四旋翼连续四次跟踪这一轨迹。100 Hz在板状态估计支持对四旋翼的实时反馈控制.

地面轨迹是用OptiTrack 5获得的。总弹道长度为61.97 m。最终漂移为[0.08，0.09，0.13]m，结果为0.29%的位置漂移。平移和旋转的细节以及它们相应的误差如图23所示。

E. 应用二：移动设备

我们将vins-Mono移植到移动设备上，并提供一个简单的AR应用程序来展示其准确性和鲁棒性。我们将我们的移动实现命名为vins-Mobile6，并将其与GoogleTangoDevice 7进行了比较，后者是移动平台上商业上最好的增强现实解决方案之一。

VINS-Mono运行在iPhone7Plus上。我们使用iphone采集的30 Hz图像，分辨率640×480，以及内置的InvenSenseMP67B 6轴陀螺仪和加速度计获得的100 Hz IMU数据。如图24所示，我们将iphone与一个启用Tango功能的联想phab 2 Pro一起安装。Tango装置使用全局快门、鱼眼相机和同步IMU进行状态估计。首先，我们在平面上插入一个虚拟立方体，该虚拟立方体是从估计的视觉特征中提取出来的，如图25(a)所示。然后，我们拿着这两个装置，以正常的速度在房间内外行走。当检测到回路时，我们使用四自由度姿态图优化.消除x,y,z和偏航漂移。

有趣的是，当我们打开一扇门时，Tango的偏航估计会跳到一个很大的角度，如图25(b)所示。其原因可能是由于不稳定的特征跟踪或主动故障检测和恢复而导致的估计器崩溃。然而，vins-Mono在这个具有挑战性的案例中仍然工作得很好。行走了了大约264米后，我们回到起点。最后的结果可以在图25(c)中看到，Tango的轨迹在最后一圈会漂移，而我们的vins会回到起点。通过对四自由度位姿图的优化，消除了总轨迹的漂移.这也证明了，与开始相比，立方体被标记到图像上的同一位置。

诚然，Tango比我们的实施更准确，尤其是对于局部状态的估计。实验结果表明，该方法可以在通用移动设备上运行，并且具有比较特殊工程设备的潜力。实验还证明了该方法的鲁棒性。视频可以在多媒体附件中找到。

X. 结论和未来工作

本文提出了一种鲁棒、通用的单目视觉惯性估计器.我们的方法既具有IMU预积分、估计器初始化和故障恢复，在线外部校准，紧耦合视觉惯性校正，重新定位，和有效的全局优化，是先进的和新颖的解决方案。我们通过与最先进的开源实现和高度优化的行业解决方案进行比较，显示出更好的性能。我们开放个人电脑和iOS的实现，以造福社会。

    虽然基于特征的VINS估计器已经达到了现实部署的成熟程度，但我们仍然看到了未来研究的许多方向。单目VINS可能会根据运动和环境而达到弱可观测甚至退化的状态。我们最感兴趣的是在线方法来评估单目vins的可观测性，以及在线生成运动规划来恢复可观测性。另一个研究方向是将单目VINS大规模部署在大量消费设备上，例如移动电话。这一应用要求在线校准几乎所有传感器的内部和外部参数，以及在线识别校准质量。最后，我们感兴趣的是制作由单目vins给出的稠密地图。我们在[44]中首次给出了用于无人机导航的单目视觉-惯性稠密地图。然而，为了进一步提高系统的精度和鲁棒性，还需要进行广泛的研究。

参考：

https://blog.csdn.net/pancheng1/article/details/81008081