一、前言

视觉里程计是通过分析一系列图像来确定相机的位置和方向的过程。视觉里程计用于各种应用，例如移动机器人、自动驾驶汽车和无人机。此示例说明如何根据一系列图像估计单个校准相机的轨迹。

此示例演示如何从一系列二维视图估计校准相机的轨迹。此示例使用筑波大学 CVLAB 创建的新筑波立体数据集中的图像。（https://cvlab.cs.tsukuba.ac.jp）。该数据集由使用计算机图形生成的合成图像组成，并包括地面实况相机姿势。

如果没有其他信息，单眼相机的轨迹只能恢复到未知的比例因子。移动机器人或自动驾驶汽车上使用的单目视觉里程计系统通常从另一个传感器（例如车轮里程表或GPS）或场景中已知大小的物体获取比例因子。此示例根据基本实况计算比例因子。

该示例分为三个部分：

1. 估计第二个视图相对于第一个视图的姿势。通过估计基本矩阵并将其分解为相机位置和方向来估计第二个视图的姿势。

2. 使用全局束调整自举估计相机轨迹。使用极性约束消除异常值。查找从前两个视图和当前视图三角测量的点之间的 3D 到 2D 对应关系。通过求解透视 n 点 （PnP） 问题来计算当前视图的世界摄像机姿势。估计相机姿势不可避免地会导致误差，误差会随着时间的推移而累积。这种效应称为漂移。为了减少漂移，该示例使用束调整优化了到目前为止估计的所有姿势。

3. 使用窗口束调整估计剩余相机轨迹。对于每个新视图，优化所有姿势所需的时间都会增加。窗口丛平差是一种通过仅优化最后 n 个视图而不是整个轨迹来减少计算时间的方法。通过不为每个视图调用捆绑包调整，进一步减少了计算时间。

二、读取输入图像序列和地面实况

此示例使用筑波大学 CVLAB 创建的新筑波立体数据集中的图像。

三、创建包含序列的第一个视图的视图集

使用对象来存储和管理与每个视图关联的图像点和相机姿势，以及视图对之间的点匹配。

转换为灰度且不失真。在此示例中，不失真不起作用，因为图像是合成的，没有镜头失真。但是，对于真实图像，不失真的是必要的。

四、绘制初始相机姿势

根据新筑波数据集中的地面实况数据创建两个图形相机对象，表示估计和实际相机姿势。

五、估计第二个视图的姿势

从第二个视图中检测和提取特征，并使用帮助程序检测和匹配特征将它们与第一个视图进行匹配。估计第二个视图相对于第一个视图的姿势。

第二个视图相对于第一个视图的位置只能恢复到未知的比例因子从地面实况计算比例因子，模拟外部传感器，该传感器将用于典型的单目视觉测程系统。

六、使用全局束调整自举估计相机轨迹

查找从前两个视图三角测量的世界点与当前视图中的图像点之间的 3D 到 2D 对应关系。使用约束的匹配项，然后对前两个视图中的三维点进行三角测量，并在当前视图中查找相应的二维点。

通过求解透视 n 点 （PnP） 问题，计算当前视图的世界摄像机姿势。对于前 15 个视图，使用全局束调整来优化整个轨迹。对有限数量的视图使用全局束调整来估计相机轨迹的其余部分，并且成本并不高。

七、使用窗口束调整估计剩余相机轨迹

通过使用窗口束调整来估计剩余的相机轨迹，以仅优化最后 15 个视图，以限制计算量。此外，不必为每个视图调用束调整，因为与 3-D 点相同的单位计算姿势。本节调用每个第 7 个视图的捆绑调整。窗口大小和调用捆绑包调整的频率是通过实验选择的。

八、总结

此示例展示了如何从一系列视图估计校准的单眼相机的轨迹。请注意，估计的轨迹与实际情况不完全匹配。尽管对相机姿势进行了非线性优化，但相机姿势估计中的误差会累积，从而导致漂移。在视觉里程计系统中，这个问题通常通过融合来自多个传感器的信息和执行闭环来解决。

九、程序

使用Matlab R2022b版本，点击打开。（版本过低，运行该程序可能会报错）

程序有偿获取：评论区下留言，博主看到会私信你。