SLAM初学-视觉里程计（一）
date
Apr 11, 2022
Last edited time
Mar 27, 2023 09:00 AM
status
Published
slug
SLAM初学-视觉里程计（一）
tags
SLAM
summary
type
Post
Field
Plat
视觉里程计（一）
2D-2D: 对极几何
对极约束
我们希望求取两帧图像 , 之间的运动，设第一帧到第二帧的运动为 , 。两个相机中心分别为 , 。现在，考虑 中有一个特征点 ，它在 中对应着特征点 。我们晓得这俩是通过特征匹配得到的。
如果匹配正确，说明它们确实是同一个空间点在两个成像平面上的投影。这里我们需要一些术语来描述它们之间的几何关系。首先，连线 和连线 在三维空间中会相交于点 。这时候点 , , 三个点可以确定一个平面，称为极平面（Epipolar plane）。 连线与像平面 , 的交点分别为 , 。, ，称为极点（Epipoles）， 被称为基线（Baseline）。称极平面与两个像平面 , 之间的相交线 , 为极线（Epipolar line）。
公式推导
设 的空间位置为：
使用齐次坐标, 写出两个像素点 的位置:
取:
带入上式:
两边同时左乘
然后，两侧同时左乘
观察等式左侧， 是一个与 和 都垂直的向量。把它再和 做内积时，将得
到 。因此，我们就得到了一个简洁的式子：
重新代入 , ，有：
- 对极约束
这两个式子等价, 都称为对极约束. 我们把中间部分记作两个矩阵：基础矩阵 （Fundamental Matrix） 和本质矩阵 （Essential Matrix），可以进一步简化对极约束：
本质矩阵
- 本质矩阵 的性质
- 本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对 乘以任意非零常数后，对极约束依然满足。我们把这件事情称为 在不同尺度下是等价的。
- 根据 ，可以证明，本质矩阵 的奇异值必定是 的形式。这称为本质矩阵的内在性质。
- 由于平移和旋转各有三个自由度，故 共有六个自由度。但由于尺度 等价性，故 实际上有五个自由度。
本质矩阵奇异值为 的证明
由 , 旋转矩阵 为正交矩阵
设
求 的特征值
由于 在不同尺度下是等价的, 我们令 , 则
由于对
- 本质矩阵的求解
- 对于任意一个 ，存在两个可能的 与它对应
只考虑它的尺度等价性，使用八对点来估计 E——这就是经典的八点法.
求解过程略
单应矩阵
- 描述了两个平面之间的映射关系. 若场景中的特征点都落在同一平面上（比如墙，地面等），则可以通过单应性来进行运动估计。
- 单应性在 SLAM 中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩 阵的自由度下降，这就出现了所谓的退化（degenerate）。 为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵 和单应矩阵 ，选择重投影误差比较小的那个作为最终的运动估计矩阵。
代码
三角测量
在单目SLAM中,仅通过单张图像无法获得像素的深度信息,我们需要通过三角测量(Triangulation)(或三角化)的方法估计地图点的深度.
三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。
现在已知 , 我们想求解两个特征点的深度 。对上式两边左乘一个
该式左侧为零，右侧可看成 的一个方程，可以根据它直接求得 。有了 ， 也非常容易求出。于是，我们就得到了两个帧下的点的深度，确定了它们的空间坐标。当然，
由于噪声的存在，我们估得的 ，不一定精确使上式为零，所以更常见的做法求最小二乘解而不是零解。
三角测量的条件
三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈的上三角测量。因此，纯旋转是无法使用三角测量的，因为对极约束将永远满足。在平移存在的情况下，我们还要关心三角测量的不确定性，这会引出一个三角测量的矛盾。
在增大平移，会导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。
三角测量代码
3D-2D: PnP
PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当我们知道 n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。
当两张图像中, 其中一张特征点的 3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的 3D 位置可以由三角化，或者由 RGB-D 相机的深度图确定。因此，在双目或 RGB-D 的视觉里程计中，我们可以直接使用 PnP 估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用 PnP。
- PnP 的求解方法
- 三点对估计位姿 P3P
- 直接线性变换 DLT
- EpnP (Efficient PnP)
- UPnP
- 非线性优化 (Bundle Adjustment)
直接线性变化 DLT
考虑到某个空间点 , 它的齐次坐标为 . 在图像 中，投影到特征点 （以归一化平面齐次坐标表示）。此时相机的位姿 是未知的。与单应矩阵的求解类似，我们定义增广矩阵 为一个 的矩阵，包含了旋转与平移信息。我们把它的展开形式列写如下：
可以得到两个约束条件
为了简化表示, 定义 的行向量:
带入得到
是待求的变量，可以看到每个特征点提供了两个关于 的线性约束。由于 一共有 12 维，因此最少通过六对匹配点，即可实现矩阵 的线性求解. 这种方法称为直接线性变换（Direct Linear Transform，DLT）.
旋转矩阵的处理
在 DLT 求解中，我们直接将 矩阵看成了 12 个未知数，忽略了它们之间的联系。
因为旋转矩阵 ，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵。
平移向量比较好办，它属于向量空间。
对于旋转矩阵 ，我们必须针对 DLT 估计的 T 的左边3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由 分解完成，相当于把结果从矩阵空间重新投影到 流形上，转换成旋转和平移两部分。
P3P
P3P 是一种只使用三对匹配点的方法.
P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点为 ，2D 点为 ，其中小写字母代表的点为大写字母在相机成像平面上的投影。此外，P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 ，相机光心为 。
这里知道的是 在世界坐标系下的坐标.
首先，显然，三角形之间存在对应关系：
由余弦定理有
对上面三式全体除以 ，并且记 ，得：
记 ，有：
消去 得到
该方程最多得到四个可能的解, 可以使用验证点来计算最可能的解, 得到 的 坐标, 然后使用世界坐标系和相机坐标系的坐标来得到相机的运动 .
- P3P 的问题
- P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。
- 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。
Bundle Adjustment
前面使用的线性方法, 都是先求相机位姿, 再求空间点位置. 而非线性优化则是把他们都看成优化变量, 放在一起优化. 在 PnP 中, 这个Bundle Adjustment 问题，是一个最小化重投影误差问题.
考虑 个三维空间点 和它们的投影 ，我们希望计算相机的位姿 ，它的李代数表示为 。假设某空间点坐标为 ，其投影的像素坐标为 。
像素位置与空间点位置的关系如下：
由于相机位姿未知以及观测点的噪声，该等式存在一个误差。因此，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：