如何理解视觉SLAM视觉里程计VO：如何从图片中还原计算出相机的三维位姿算法教程Python代码实践

视觉里程计是什么？视觉里程计有什么用？需要我们求解什么？

视觉里程计（Visual Odometry,VO） 算法是什么？视觉SLAM中的视觉里程计算法就是指的是通过某个算法根据前后两张图片求出当前时刻内相机位置和姿态相对上个时刻相机位置和姿态的变化的那种算法。这个就和出租车的里程计一样记录每个时刻的车轮速度及其滚动距离类似。只不过视觉里程计算法是求相机在各个时刻的位置姿态，所以才叫做视觉里程计算法。

视觉里程计有什么用？我们如果将最开始那个时刻的相机位置当做世界座标系。现在我们使用视觉里程计算法计算出了相机各个时刻的位姿变化，这意味着我们得到了相机相对于世界座标的位置。这个有什么用呢？有两个用途：1. 定位，现在我们知道了相机相对于世界座标的位置，而相机是和机器人绑在一起的，这不就知道了机器人在哪了么？ 2. 三维重建（建图），现在很多相机是能够测出当前时刻环境中各个物体离相机自己的距离（Lidar, RGB-D和双目相机都能测距离），而视觉里程计算法是已经计算出相机自己相对世界座标的距离。这就可以计算出环境中各个物体相对世界座标的距离。这不就建好图了么？

前面已经从直观上描述了视觉SLAM的核心思想——视觉里程计算法。有了视觉里程计算法我们就能知道相机在哪（相机与机器人/汽车绑定的所以也知道了机器人/汽车在哪）。有了视觉里程计算法我们就能对环境进行三维重建。前面提到了视觉里程计算法是一种能够计算出相机在各个时刻相对于前一个时刻的位置和姿态的变化的算法。那么问题来了位置和姿态的变化到底它在数学上是如何量化的？

我们知道任何刚体的运动包含两个部分一个是旋转一个是平移。旋转的变化量对应的是旋转矩阵$\bold R$，平移量对应的是一个向量$t$。所以视觉里程计算法就是想根据前后两个时刻拍出的照片中的一些特征点之间的约束关系求$R$和$t$。

举个例子：现在某个自动驾驶的汽车上有个雷达。我们将雷达最开始开机的地方作为世界座标系。现在是开机的下一个时刻，当前时刻用雷达对环境扫描，于是我们就获得了一系列点云。注意这些点云的座标是相对于当前雷达的座标。为了对环境建图我需要计算出这些点云相对于世界座标系下的座标，然后将他们保存这就建好了地图。那么这和视觉里程计算法有什么联系呢？通过视觉里程计算法我们能计算出当前雷达相对于上个时刻的旋转矩阵$R$和平移$t$。前面提到了要想建一个地图那就得计算出扫描出的点相对于世界座标系下的座标，那么如何根据这两个数据计算出点云中各个点相对于世界座标系下的座标呢？假设点云中某个点的座标是$P$，那么这个点相对于世界座标系下的座标是$RP+t$。我们将这个结果保存就得到了地图。

我想现在你已经知道视觉里程计算法有多么重要了。也知道视觉SLAM的基本思想了（其实SLAM并不是指某种算法而是一套处理数据的流程）。也知道视觉里程计算法就是用来求出前后两个时刻相机位置和姿态的变化量。那么问题来了如何计算当前时刻相对上个时刻的位置和姿态的变化量?

如何从前后两张图片中还原计算出相机的三维位姿变化量？

$$