第七讲 视觉里程计1

根据相邻图像的信息估算相机的运动称为视觉里程计（VO）。一般需要先提取两幅图像的特征点，然后进行匹配，根据匹配的特征点估计相机运动。从而给后端提供较为合理的初始值。

1. 特征点：特征检测算子SIFT，SURF，ORB等等。

SIFT算子比较“奢侈”，考虑的比较多，对于SLAM算法来说有点太“奢侈”。不太常用目前。

ORB算子是在FAST算子基础上发展起来，在特征点数量上精简了，而且加上了方向和旋转特性(通过求质心)，并改进了尺度不变性（通过构建图像金字塔）。从而实现通过FAST提取特征点并计算主方向，通过BRIEF计算描述子的ORB算法。

2. 特征匹配：特征匹配精度将影响到后续的位姿估计、优化等。

根据提取的匹配点对，估计相机的运动。由于相机的不同，情况不同：

1.当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。2D-2D

2.当相机为双目、RGB-D时，或者我们通过某种方法得到了距离信息，那问题就是根据两组3D点估计运动。该问题通常用ICP来解决。3D-3D

3.如果我们有3D点和它们在相机的投影位置，也能估计相机的运动。该问题通过PnP求解。

分别来介绍。3D-2D

一、 对极几何

假设我们从两张图像中，得到了若干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间摄像机的运动。

通过推导可得对极约束为:

其中，P1和P2表示像素坐标，K表示相机内参，t和R表示相机运动。该等式的几何意义为三角形O1PO2共面。可以将上式中间的参数合并为一个矩阵，即本质矩阵：

 其中，x1和x2表示归一化平面坐标，当然可以将内参与本质矩阵合并得到基础矩阵：

本质矩阵

一般采用八点法求解E矩阵：

 通过本质矩阵求解相机运动R,t的过程是由奇异值分解SVD得到的：

 分解结果存在4个解，仅有两个相机中P点都是正深度才是正确的解。而一般情况下，得到的匹配特征点是远远多于8对匹配点的，所以可以采用最小二乘法或随机采样一致性求解E。

单应矩阵

描述了两个平面之间的映射关系，自由度为8的单应矩阵可以通过4对匹配特征点算出。

二、PnP

DLT

已知世界坐标系下P点坐标和在相机中投影点，则可根据下式进行求解相机运动：

 转换为线性方程为：

t有12个未知量，可通过6对匹配点，即可线性求解，超过6对时，可通过最小二乘求解。但由于t中未考虑的约束，所以t举证左边3X3的矩阵块需要进行近似化，即

P3P

仅使用3对匹配点，可求得匹配点在该相机时刻下的坐标，接下来再使用ICP即可求出相机的相对运动。

最小重投影误差 BA

非线性优化方法，将相机位姿和路标坐标放在一起进行优化，像素坐标与空间点的位置关系如下：

在pnp问题中，空间点位置已知，仅为优化相机位姿T的问题。构建最小二乘问题，寻找最好的相机位姿：

 上式二范数中可记为误差项e(x)，误差项表示3D的投影位置与观测位置做差，即重投影误差。此处，将会用到李代数进行优化，用到的知识点主要有ch4和ch6。由于优化的为相机位姿，所以误差项中的x为李代数ξ，对误差项线性化为：

 接下来，需要推导出雅可比矩阵J的具体表达形式。

空间点坐标P在相机坐标系下的坐标P’为：

 P’的像素坐标为：

 对T左乘扰动量，则e关于△ξ的求导如下：

其中，左边部分表示为误差项关于相机坐标系下坐标求偏微分，右边部分可转换为李代数求导。左边部分可表示为如下所示：

右边部分可表示为如下所示：

由此，可推到出JT为：

若se(3)旋转在前，平移在后，则需要将该矩阵前3列和后3列对调即可。

上面皆为对相机姿态T进行雅克比矩阵推导，同样也可用对路标点进行雅可比矩阵推导：

由此可推导出误差项关于路标点的雅克比矩阵：

三、ICP

假设有一组匹配好的3D点和，即可表示为：

3D-3D位姿估计问题中无需相机模型

ICP求解分为线性代数SVD和非线性优化方法BA 

 SVD

误差项可表示为：

对很多匹配点可进行最小二乘法求解位姿：

其中，，所以ICP在SVD方法下分为以下三个步骤：

1. 计算每个点的去质心坐标：
2. 在最小二乘表达式中，左边部分仅与R有关，所以可以先根据下式对R进行优化求解：

经过一系列推导可得：

设，对其进行SVD分解即可求出，若R求得的行列式为负数，则取-R为最优值。

根据计算得到了R，即可求出t：

非线性优化方法

非线性优化不断迭代求解最优值，ICP对初值的要求比较宽松，其实就是扰动模型：