第7讲 视觉里程计1

视觉里程计，也就是前端，核心是根据相邻图像的信息估计相机运动，给后端提供很好的估计值。

主要算法：

• 特征点法
• 直接法

7.1 特征点法

主流方法。

我们需要知道如何提取、匹配图像特征点，然后估计两帧之间的相机运动和场景结构，从而实现一个两帧间的视觉里程计。

7.1.1 特征点

所谓特征，就是图像里比较有代表性的点，这些点在相机小量运动之后也能保持稳定。例如角点、边缘、区块。在SLAM中，特征=路标。

• 角点特征（早期）
  • Harris角点、FAST角点、GFTT角点等
• 局部图像特征
  • SIFT、SURF、ORB等

特征点：关键点 + 描述子（提取特征点=提取关键点并计算描述子）

• 关键点：特征点在图像中的位置（大小、方向、评分）
• 描述子：描述关键点周围像素信息的向量

7.1.2 ORB特征

ORB特征：在平移、旋转、缩放变换下表现良好，同时高效

• Oriented FAST关键点提取：找出图像中的角点，计算特征点的主方向
• 改进后的BRIEF描述子：对前一步提取出的特征点的周围图像区域进行描述

1) FAST 关键点

1. FAST角点主要检测局部像素灰度变化明显的地方。

缺点：

• 重复性（相同特征点可以在不同的图像中找到）不强
• 分布不均
• 不具有方向信息
• 不具有尺度信息

---

2. ORB改进版——Oriented FAST：加上尺度和旋转的描述。

• 尺度：图像金字塔，每层检测角点
• 旋转：灰度质心法—get关键点的方向信息——几何中心指向质心

2) BRIEF 描述子

BRIEF：一种二进制描述子，用0和1来编码特征点附近两个随机像素的大小关系。

Steer BRIEF：ORB改进后的BRIEF，利用关键点的方向信息，计算旋转之后的Steer BRIEF特征。这样一来描述子也能具有旋转不变形。

• 描述子距离表示了两个特征的相似程度，对于BRIEF描述子，可以使用汉明距离。

7.1.3 特征匹配

确定当前图像路标与之前图像中的路标之间的对应关系。

主要问题：场景中的重复纹理导致误匹配。

方法：

• 暴力匹配
  • 计算每个特征点与其他特征点的描述子距离，选取最近的作为匹配点
• 快速近似最近邻（FLANN）

7.2 编程：特征提取和匹配

opencv

7.2.1 OpenCV的ORB特征

提取并匹配两种图像的ORB特征：

1. 读取图像
2. 初始化
3. 检测Oriented FAST角点位置
4. 根据角点位置计算BRIEF描述子
5. 对两张图像中的BRIEF描述子进行匹配，使用汉明距离
6. 匹配点对筛选：汉明距离小于最小距离的两倍，否则算是误匹配
7. 绘制匹配结果

   7.2.2 手写ORB特征

   编译该程序要求CPU支持SSE指令集

7.2.3 计算相机运动

已有匹配好的点对，接下来要根据点对来估计相机运动：

1. 单目：对极几何
2. 双目/RGBD：ICP
3. 一组3d一组2d：PnP

需要注意的是，这里的匹配点对是来自两帧的，估计的相机运动也是两帧之间的相机运动。

7.3 2D-2D：对极几何

已知匹配的投影点对位置，求相机的位姿（R, t）。

7.3.1 对级约束

• l1, l2：极线
• e1, e2：极点
• O1O2P：极平面
• O1O2：基线

推导得到对级约束：其中x1，x2是像素点的归一化平面坐标

对级约束的几何意义是O1，P，O2三者共面。

进一步化简对级约束：

• 基础矩阵F
• 本质矩阵E

• 对级约束给出了两个匹配点的空间位置关系

相机位姿估计问题：

• 根据匹配点的像素位置（p1, p2）求出E或者F （内参K已知情况下，主要用E）
• 根据E或者F求出R,t

7.3.2 本质矩阵E

E=t^R

E具有尺度等价性，实际上由5个自由度。

八代法估计E：8对匹配点组成矩阵，满足秩为8的条件，解方程得到E

---

根据E求R,t：奇异分解SVD

得到4个可能解，只有一个解对应的P在两个相机中都具有正的深度，是正确解

7.3.3 单应矩阵

单应矩阵H：描述了两个平面之间的映射关系，通常用于描述处于共同平面上的一些点在两张图像之间的变换关系。

1对匹配点构造两项约束，4对匹配点可以得到8项约束，计算出8个自由度的单应矩阵：

解得H，分解（数值法、解析法）后得到R,t。

• 单应性在SLAM中的应用：if 特征点共面/相机发生纯旋转，基础矩阵的自由度下降，即退化，这种时候如果还用8点法求解基础矩阵，多余的自由度将会主要由噪声决定。这种时候我们可以同时估计基础矩阵F和单应矩阵H，选择重投影误差较小的作为最终的运动估计矩阵。

7.4 编程：对级约束求解相机运动

opencv

单目初始化不能只有纯旋转，必须有一定平移。

随机采样一致性RANSAC：当数据带有错误匹配时可以用

7.5 三角测量

通过对极几何得到相机的运动R,t之后，需要用这个运动估计特征点的空间位置。

但是在单目SLAM里，单张图片无法获取像素的深度信息，需要用三角测量法来估计地图点的深度。

也就是说，目前已知R,t，想求解两个特征点的深度s1和s2.

三角测量：在不同位置对同一个路标点（特征点）进行观察，从观察到的位置推断路标点的距离。

本来两条直线的交点P就是空间点了，但是由于存在噪声，这两条直线往往无法相交，可以通过最小二乘来求解。

7.6 编程：三角测量

7.6.1 三角测量代码

之前对极几何求解的相机位姿 + 三角化，得到特征点的空间位置。

7.6.2 讨论

纯旋转不能用三角测量，故如果相机原地旋转导致视差很小，可能出现追踪失败、尺度不正确等问题。

视差问题：三角化存在一个矛盾点，增大平移，可能导致匹配失效，平移太小，导致三角化精度不够。

延迟三角化。

7.7 3D-2D：PnP

2D-2D对极几何缺点：

• 需要8+点对
  • 如果3D-2D，只需要2+1点对
• 初始化问题
• 纯旋转问题
• 尺度问题

因此，双目/RGBD直接用PnP估计相机运动，单目先初始化再用PnP。

PnP：

• 已知3D点的空间位置和它在相机上的投影点，求相机的外参（R, t）【大多数情况】
• 已知相机坐标A中的3D点位置，和相机B中的2D点位置，求两个相机的相对运动，（和2D特征点的空间位置）

PnP求解方法：

• 代数法
  • P3P
  • 直接线性变换DLT
  • EPnP
  • UPnP
• 优化法
  • 非线性优化——BA

7.7.1 直接线性变换DLT

已知空间点P坐标和它的投影点x1，估计相机位姿R, t

1个特征点（或者说1对3D-2d匹配点）可以提供2个关于t的线性约束，6对匹配点提供12个约束，可以求解12个未知t。

求出来T矩阵，但是左边3X3的矩阵块不一定满足旋转矩阵R的约束，因此要用QR分解找一个最好的旋转矩阵来对它进行近似：

7.7.2 P3P

上面的DLT用了6对匹配点，这里的P3P只需要用4对，3个来算，1个验证。

已知3个3D点ABC（已知其世界坐标系下的3D坐标），和匹配的3个2D点abc，估计相机外参。

思路是利用几何性质，将3D-2D转换成3D-3D问题再求解：

• 转换

已知：

未知：

可以用吴氏消元法+验证点解得a,b,c在世界坐标系下的3D坐标【这里书上写的是ABC在相机坐标下的3D坐标，感觉不太对】，然后就变成了3D-3D问题。

• 求解3D-3D：ICP

7.7.3 最小化重投影误差求解PnP

• 线性方法：先求相机位姿，再求空间点位置（如2D-2D对极几何+三角化）
  • 线性方法利用多个匹配点来拉平噪声造成的误差？
  • 【疑问】：线性方法有没有考虑误差的影响？？？
• 非线性方法：将相机位姿和空间点放在一起来优化——Bundle Adjustment
  • 非线性方法直接对实际和估计的误差进行最小化

前面也说过PnP的2D-3D来源可以是两种：相机在某个位姿下将空间的3D点投影到2D平面，这种情况下我们只要估计相机位姿就行；另一种跟2D-2D有点像，两个匹配点分别来自两帧，只是一个是3D的一个是2D的，这种时候除了估计相机位姿，还要估计2D点的空间位置。

---

• 优化位姿

下面讲解如何在PnP中构建一个BA问题来对相机位姿进行优化：

已知空间点P及其投影p，目标是估计相机位姿R，t。由于相机位姿未知且存在噪声，su=KTP存在误差，构建最小二乘问题，寻找最好的相机位姿来最小化误差。该误差项是3D点的投影位置和观测的投影位置作差，故被称为重投影误差：

高斯牛顿/阻尼牛顿来求解，重点是J（重投影误差关于相机位姿对应的李代数的一阶导数）

• P在世界坐标系下：[X,Y,Z]^T
• P在相机坐标系下：P’=(TP)=[X’, Y’, Z’]^T
• P’的投影(u,v)：su=KP’

J 的求解可以参考”如何使用扰动模型来求李代数的导数”：

第一项是误差关于变换后点的导数：

第二项是变换后的点关于李代数的导数：

最终得到雅克比矩阵J：

---

• 优化特征点的空间关系：

这里J是重投影误差关于空间点P的一阶导数：

第一项如上：

第二项：P’=RP+t，求导后只剩下R

最终得到雅克比矩阵J：

7.8 编程：求解PnP

7.8.1 使用EPnP求解位姿

线性优化

OpenCV中的EPnP

7.8.2 手写位姿估计

非线性优化

手写高斯牛顿法的PnP

7.8.3 使用g2o进行BA优化

非线性优化

调用g2o

第一个相机的位姿固定为0，根据1组3D点和第2个图像中的2D投影来估计第二个相机位姿。

7.9 3D-3D：ICP

已知一组匹配好的3D点P,P’，求解P’——>P的欧式变换R,t。

值得注意的是，3D-3D的位姿估计问题中没有相机模型，因为两组3D点变换与相机没有关系。但是，如果将这两组点集分别看做两个帧图像的数据，那么这里的R,t就是两帧间的相机位姿了。

迭代最近点ICP：

• 激光SLAM中的ICP，两个点集之间并无匹配关系，因为激光数据的特征不丰富，因此只能认为距离最近的两个点为同一个点
• VSALM中的ICP，两个点集是匹配好的

ICP求解：

• 线性法：SVD
• 非线性优化：BA

7.9.1 SVD方法

构建最小二乘问题：最小化点集中各对点的误差项之和

定义质心后，简化后的最小二乘问题：

思路是最小化第一项，取t让第二项为0。

步骤：

再化简这个优化目标函数，最终相当于要优化：

通过SVD可以解出上述问题中最优的R：

再求t就行。

7.9.2 非线性优化方法

BA法以迭代的方式最小化目标函数：

然后用高斯牛顿等方法不断迭代找到极小值就行。

点集匹配时求解ICP问题有个优势，是可以任意选定初始值（因为唯一解情况下极小值就是全局最优解）。

但是事实上，如果特征已经匹配好了，往往使用线性方法直接解（因为存在解析解），而不是迭代。也就是说，ICP更多使用在匹配未知的情况下。

7.10 编程：求解ICP

7.10.1 编程：SVD方法

Eigen调用SVD解出目标函数的解析解。

7.10.2 编程：非线性优化方法

---

第8讲 视觉里程计2

特征点法缺点：

• 关键点的提取与描述子计算非常耗时
• 只使用特征点会丢失大部分也许有用的信息
• 特征缺少的场景下匹配点不够计算相机运动

8.1 直接法的引出

改良方案：

• 光流法：保留特征点，只计算关键点，不计算描述子，将匹配描述子的步骤替换成光流跟踪，即使用光流法跟踪特征点的运动。
  • 降低计算开销
  • 只改了特征匹配的方案（改为跟踪），但还是用对极几何、PnP或ICP来估计相机运动
• 直接法：不再使用特征点，只计算关键点，不计算描述子。同时使用直接法计算特征点在下一时刻图像中的位置。
  • 直接法不要求提取到的点是角点，只需要根据图像的像素灰度信息来估计相机运动和点的投影。
  • 进一步降低计算开销
  • 全改，不需要匹配特征，直接最小化光度误差

光流法也算是特征点法的一种。

直接法是从光流法演变而来的。

可用于的结构：

• 直接法：
  • 稠密
  • 半稠密
  • 稀疏
• 特征点法：
  • 稀疏

8.2 2D光流

1) Lucas-Kanade光流

8.3 编程：LK光流

8.3.1 使用LK光流

8.3.2 用高斯牛顿法实现光流

8.3.3 光流实践小结

8.4 直接法

8.4.1 直接法的推导

8.4.2 直接法的讨论

8.5 编程：直接法

8.5.1 单层直接法

8.5.2 多层直接法

8.5.3 结果讨论

8.5.4 直接法优缺点总结