3月9日 笔记：RANSAC随机样本一致性,灭点、对极几何计算、H矩阵、PNP估计相机位置，3D匹配、投影变换、N点定位求解姿态

【修订】：
2020-03-11：特征匹配 分为：（1）2D-2D ； （2）2D-3D ； （3）3D-3D
https://www.zhihu.com/question/51510464

• 2D-2D
  在2D-2D情况下，你只有两个点的2D坐标
  (1)对极约束（Epipolar Constraint）。简而言之，随便你出一组匹配点，都会有这么个约束成立。从配对点算E(Essential（本质）矩阵)。然后就是用E算R,t的问题了。另外再啰嗦一句，当你不知道内参时，只有像素坐标，会引入F（Fundamental，基本矩阵），和E大同小异但是性质比E麻烦点。因为通常认为相机已经标定好了所以也用不着它了。
  （2）单应H矩阵。有一种情况是你找的那些点都位于一个平面上，比如说你的相机是朝天花板或地板看的，这时候分解E和F会出现退化，要用单应H矩阵来解。用给定的一堆匹配点算H；用H算出R,t,n,d（要用到SVD和QR分解。最后你会得到八组解，然后有一串步骤告诉你如何从这八组解里选出最好的）。
  总之，要知道，在特征点位于平面上时，分解H；否则分解E或F。

稍微说几句。2D-2D的情况换言之，在分解过程中，对乘以任意非零常数，分解都是成立的，这个叫做单目SLAM的尺度不确定性。因此，我们通常把t进行归一化，让它的长度等于1。或者让场景中特征点的平均深度等于1，总之是有个比♂例的。此外，分解E的过程中，如果相机发生的是纯旋转，导致t为零，那么，得到的E也将为零。于是，另一个结论是，单目初始化不能只有纯旋转，必须要有一定程度的平移！

• 3D-3D，ICP：
  你不光得到了匹配点，还知道这两组匹配点的深度，于是有了3D-3D的匹配。因为你知道匹配，这种情况下 R,t 的估计是有解析解（闭式解）的。否则，如果只有两堆点而不知道匹配，则要用迭代最近点（Iterative Closest Point, ICP）求解。

• 3D-2D，PnP

• PnP（Perspective n Points）就是你有n个点的3D位置和它们的投影，然后要算相机的位姿。这个倒是SLAM里最常见的情况，因为你会有一堆的地图点和像素点等着你算。PnP的做法有好多种：直接线性变换，P3P，EPnP，UPnP等等，基本你去找OpenCV的SolvePnP中的参数即可，好用的都在那里。除此之外，通常认为线性方程解PnP，在有噪声的情况下表现不佳，所以一般以EPnP之类的解为初始值，构建一个Bundle Adjustment（BA）去做优化。

1.0 参数求解问题另一种思路RANSAC： Random Sample Consensus 随机样本一致性

Strategy:
To find a model that accords with the maximum number of samples 。

Assumptions:

1. Majority of good samples agree with the underlying model (good apples are same and simple.).
2. Bad samples does not consistently agree with a single model (all bad apples are different and complicated.).

假设：1.多数好的样本与基本模型一致（好的苹果是相同且简单的）。
2.不良样本不能始终与一个模型一致（所有不良苹果都是不同且复杂的）。

对于参数求解问题还有另外一种思路：RANSAC算法。它与最小二乘各有优缺点：

• 当测量值符合高斯分布（或者说测量误差符合期望为0的高斯分布），使用最小二乘比较合适，可以获得比较稳定且很高的精度。而当误差服从高斯分布的情况下， 最小二乘法等价于极大似然估计。

• 当测量值离散性比较大，存在很多outliers，那么使用最小二乘求解就会存在很大的误差，此时使用RANSAC算法更合适。

• 线性最小二乘只适用于参数模型会线性关系的情形，RANSAC则没有此限制。

1.1问题引入

在最小二乘中，如果存在非常奇怪的点，会对模型准确性产生巨大的影响。

在上面提到了，采用Least Square+SVD的方法可以十分有效地解决问题。Least Square用于构造方程组，SVD则用于解方程组。但这里有个问题，最小二乘方法虽然很棒，但是对离群点十分敏感，可能个别离群点会导致解的不稳定，如上图所示。因此在应用最小二乘之前应该需要采用RANSAC来去除离群点。Random Sample Consensus，随机采样一致性。是去除离群点(Outlier)的好办法。

1.2过程

为了去除离群点，我们使用这样的策略：找到一个适合绝大多数样本的模型，对于离群点则选择忽略。


选择一个内部数的概率：

建立正确模型的可能性： $W^n$，其中，n是建立模型的样本数。
在k次迭代中未建立正确模型的可能性 $(1-w^n)^k$
$(1-w^n)^k=1-p$， 其中p是期望的RANSAC成功率。

2.0Where am I？由成像来预估相机的角度

这里，根据传感器形式的不同，分成三种情况：
你用的是单目相机，于是只有2D-2D的配对点；
你用的是RGBD或双目相机，于是你有3D-3D的配对点；
你只知道一张图中3D信息，另一张图只有2D信息，于是有3D-2D的配对点。

2.1回忆1：Vanishing Point Based Method

通过影像中真实世界的两个垂直方向上的灭点即可以恢复相机相对于他们所在平面的姿态。

我们再回到这个街角的例子， 在这里我们可以看到两条街和一栋建筑物和一个角落 。如果能识别出图像中的街道 ，我们知道道路是， 由两组直线构成的， 这两组线将会聚至无限距离上 的一点， 也就是我们看到的成像 这就是无限距离上的一点

一旦我们确定了无线距离上 点的位置， 通常通过确定在成像上， 光线相交的一个单点 。我们得到这个点 在图像像素空间内与我的光射线共同组成三维空间内的一束光线。

同时这条由光心向空间内部

图像上的灭点移动的光线 事实上就与我们在物理空间 中观察到的这条街道平行

因此 如果我已知这条光射线 我们就能准确地得到 z方向上街道的朝向 这就使我们可以计算出 这两条轴的定向角 在这样的情况下 我们就可以 得到相机的旋转和倾斜角度。

过程

回忆一下我们总是 需要将像素坐标

通过相机校准 也就是K 转换回光学世界中 那是K的逆矩阵乘上灭点 等于光射线 并且这条光射线在 z轴或者说是三维空间内的 街道轴所对应的方向上

我们把矢量单位化 这样一来得到基数1 通过它我们可以估测 出两个旋转的角度

所以我们该怎么 得到三个旋转角度呢? 因为这要求我们 观察在三维空间中

由两条垂直光线 得到的两个灭点 所以我们必须在物理 空间中找到两条光线 并且它们互相垂直

那么这两束垂直的光线 就一定构成了这图像 通过在图像中的两个灭点

所以同样的方法可以 确定图像中的两个灭点 计算其像素坐标 通过K的逆矩阵将像素 坐标转换为光学列阵 然后单位化它们 构成两条旋转轴

通过取两个旋转轴的叉乘积 我们得到了完整的 三维旋转矩阵

因此我们就知道了相机的方向

2.2 回忆二：Epipolar Geometry Based Method

除了在单张影像中使用灭点，还可以在两张影像间利用对极几何计算相机相对位姿。

另一种方式来计算相机的朝向 那就是当你有两个 朋友 同时对同一个 三维空间中的物体拍照时 而他们又愿意与对方分享照片

所以通过这两幅图本身我们 就可以算出它们之间的对极几何 通过找到它们上 八个相对应的点

这样一来这个对极几何 就能让我们确定这两个视角 之间的平移和旋转关系

2.3 方法3：Homography Based Method

对于真实世界中位于某一平面上的物体，如果我们知道它们的一些物理属性(实际大小)，当我们使用相机拍摄它们时，就可以利用单应投影(Homographic Projection)来求得拍摄时相机相对于该物体所在平面的位姿。事实上，对于物体的实际大小，是很容易知道的。如标定板格网大小、人造物体：门、窗等。

需要说明的是，在一幅影像中可以存在多个平面，选择不同的平面进行单应矩阵求解，很显然最后求得的单应矩阵也是不同的。表示相机拍摄时刻相对于不同平面的位姿，如相对于墙壁、相对于地板等。这是十分灵活的，不要“死死地”认为两张影像间只能求一个单应变换。

如上图所示，右图为真实世界中的标定板的俯视图。在其中有一个已知点X1，假设它在标定板平面内的坐标为(u1,v1)，写成齐次形式，末尾加1，为(u1,v1,1)。某相机拍摄了它的照片，如左边所示，容易测得对应X2的像素坐标为(u2,v2)，写成齐次形式为(u2,v2,1)。这样我们可以通过一个3×3的单应矩阵H实现单应映射(Homography Mapping)。仔细观察H可以发现它联系的是真实世界的坐标与像素坐标，从物理意义上说也就是相机的内参矩阵K、外参(旋转R、平移t)都包含在了这9个元素中。因此只要能利用对应点对求解H，就可以求得相机相对位姿。

这个3乘3的H矩阵 拥有9个元素

事实上H矩阵内就包含着这两个 相机校正矩阵k和旋转矩阵r 还有平面和相机 中心之间的平移t

但是它们都被缩减为这个紧凑 的只有9个元素的H矩阵结构

已知一点对应 三维空间中的一点 和图像空间内的一点 我们可以 得到这个单应映射矩阵 x2=Hx1 。x1和x2的值都是已知的 都有三维上的数据 H是我们想要估测 的量 用橙色来表示。

我们需要做的第一步
是得到一个 等号右边为0的 且H的线性映射在等号 左边的这么一个方程

这可以通过取x2与 其自身的叉乘积 来轻松地得到，所有的矢量与其 自身相叉乘都等于0

所以在等式的左边我们有一个0 在右边我们有x2 叉乘矢量Hx1 = 0

所以我们将继续：
在不同的对应点上这么做 ，每一次我们这么做都会得到一组 方程 ，在其等是左边我们有未知数H 同时已知的0在右手边 将这个方程一点点地展开来

我们用u2 v2来表示 图像内x2的像素坐标

所以我们将继续：
同时我们将H分开 表示 h1表示第一行 h2表示第二行 h3表示第三行

我们将矢量x1与H相乘 进一步展开这个方程 变成这里表示的这样

为了能达到在视觉 上简洁干净 我们将 h1 h2 h3 用橘色来标记 他们都是1乘3的行矩阵 同时x1在各列中被复制了 所以这个橙色矢量 和灰色矢量将产生一个标量

所以我们将继续：
这样我们就可以在不改变其值 的情况下取这个标量的转置矩阵 这就相当于取竖直的列矢量x1 将其转化为水平形式 将其转置 同样的将h2 水平矢量转化为 垂直矢量 h1转置矩阵

所以说什么都没有变 他们 还是会为我产生三个标量矢量

从数学的角度来说 它可以 这样被更清楚地表示 第一行是x1的转置 矩阵乘h1的转置矩阵
所以我们将继续：
接下来我们取矢量u2 v2 1 把它以叉乘形式展开来

矢量的叉乘积可以被表示为 一个反对称的矩阵写 在左边 用绿色表示

我们继续取x1转置 和h1转置的点乘积 并进一步将其展开

所以我们将继续：
因为一个矩阵矢量乘矩阵包含 三行x1的转置矩阵 它们 分布在这条对角线上 而剩下的位置则都是0 如中间这图所示 而h将再一次被表示为列矢量 其中前三个项就是h1的第一行 接下来的三个项是h2的第二行 而最后的三个项 是h3的最后一行

将这些合起来我们就会发现 我们得到了一个这样形式的矩阵 ，包含了x1 x2的乘积 图像内图像坐标的位置 和平面上的位置x1

矩阵最终的尺寸是3行9列 因为每一个矢量x1的转置 矩阵都是一个三维度的矢量 你有3个这样的矢量 所以总共有9列数据 也就是这里的紫色 矩阵 3乘9矩阵 将其乘以橘色列矢量h1 也就是包含 所有已知h值得矢量 的结果为0 现在我们可以得到一个 我们比较熟悉的矩阵 也就是我们前面讨论过的在 最小平方环境内我们有的Ax = 0
$Ax=0$

参考：

求解单应矩阵的过程如上图所示。X1为齐次的真实世界坐标，X2为齐次的像素坐标，橙色的H为未知量。

看到这个式子“本能的”反应是方程两边同乘以X1的逆不就得到H了吗？但仔细一看会发现这里X1是向量，还是3×1的，没办法求逆(非方阵没有逆矩阵，只能求伪逆)。

所以直接乘以X1的逆的想法就不行了。所以这里使用了个小技巧：等式两边乘以X2的叉乘，左边X2与自身做叉乘结果为0向量，右边则是乘上X2的反对称矩阵(3×3)，它是一种固定格式的矩阵。

在后续式子中，为书写方便不妨将H矩阵按行拆分成h1、h2、h3，大小都是1×3。这样，根据矩阵乘法法则HX1其实可以写成h1、h2、h3分别与X1相乘，每一行得到一个标量，3行得到一个3×1的向量。

写成这样以后可以使用第二个小技巧：根据矩阵转置法则 $(AB)^T=B^TA^T$，对AB转置，结果不变，但AB的位置交换了。

例如 $(1 2 3)与(4 5 6)^T相乘结果等于(4 5 6)乘(1 2 3)^T$。

我们希望未知量都在最右边(构造Ax=0的形式)，所以我们对HX1取转置，就可以得到 $X1^TH^T$，实现了H与X1的位置互换。用h1、h2、h3表示就是图中第一行最右边的式子，这样我们就成功将H移到了最右边。

但现在还有个问题，X1与H是“粘”在一起的，我们要想办法将它们分离。利用线代的知识，可以很容易把X1改写成一个“对角阵”(之所以加引号是因为它的形状并不是严格的对角阵，只是类似)，对角线元素即为X1^T，它的形状为3×9，这样便成功实现了“分离”。

最后将它与前面X2的反对称矩阵相乘，即可得到3×9的系数矩阵A，如图中最下面一行的紫色部分所示。

这样每一对点都可以写出上面这种形式的方程组，多对点，如4对时候的A矩阵大小即为12×9。

已知一个单点 我们 可以获得这一系列 方程式 Ax = 0 其中 A是一个3乘9的矩阵 同时我们知道这个3乘9矩阵 的秩实际上是2 因为我们 只得到了两个x y的量

而每个矩阵都有9个项 但是我们可以用各种 方式来按比例变化它 所以有一成的 自由度我们可以移除 所以总共是9减1个项 也就是8个需要估测的项

因此 如果我在三维 空间内的平面到图像 有四个相对应的点 我们就能在这个线性系统上 获取足够的条件 来确定H的项的值 当然 如果我们有更多的点 我们就会有更多的条件 这就会产生最小二乘问题

一旦我们得到了最小平方解我们就可以通过寻找a的最小 本征向量来获得对于H的解

SVD分解

而对于多于4对点的情况，在就可以用最小二乘方法解决了，具体办法是对A进行SVD分解，得到的V的最右边一列即为结果。

我们取a的奇异分解值 将其转换为ud v的转置矩阵

这样我们就可以将v的 最后一列 也就是v的第9列 也就是重组后的H矩阵 我们将这个H矩阵矢量 转化为一个矩阵形式 也就是一个3乘3矩阵 这使得我们能够获取 几个旋转和平移的值.

H矩阵转化得到R和T

这里需要注意的是，正如前面提到的，H包含了相机内参矩阵K以及外参R、t，所以要想获得外参还需要先把内参去掉。不含内参的H应该在采用数值解法求得的H左边乘以 $K^{-1}$。这样得到H以后，H的第一列对应r1、第二列对应r2、第三列对应t。对r1按照H第一列列向量归一化，得到的就是R的第一列，r2等于H第二列除以第一列列向量模长，r3等于r1×r2，t等于H第三列除以H第一列列向量模长。根据公式可恢复出相机的旋转与平移。

首先我们再一次通过H的逆变换 将H转换入光学空间中，已知变换矩阵H的前两列 事实上 第一列来自其旋转矩阵 接着是来自旋转矩阵的 第二列 接下来才是转换矢量t，现在我们将其重整来 得到一个合适的旋转矩阵 来确保第一列中r=1 也就是其基数为1。这可以用这里的 一系列方程来表示

这里一定要注意：并不是所有的单应矩阵都包含内参矩阵K。其实根据前面学习的定义，单应矩阵映射了三维空间中两个不同平面上的点的变换关系。是一个数学上纯理论的概念，与相机内参矩阵K毫无关系。例如在第二周的作业中求解单应矩阵时，给定两张影像中的4对对应像素坐标即可以求得平面区域的单应变换关系，直接构造Ah=0方程，SVD分解就得到结果了，与内参矩阵K无关。之所以在这里H包含了K是因为这里单应矩阵映射了像素坐标与真实世界坐标的关系，而将像素坐标转成三维坐标是需要内参矩阵K的，所以单应矩阵就把K给“吸收”了进来。这一点需要注意和理解。

拓展：反对称矩阵

参考：https://blog.csdn.net/qq_35043589/article/details/78878850
定义
设A为n维方阵，若有A′=−A,则称矩阵A为反对称矩阵。

对于反对称矩阵，它的主对角线上的元素全为0，而位于主对角线两侧对称的元素反号。

向量的反对称矩阵

参考：

2.4 （perspective n point）PnP Based Method：不是在同一平面上的点，如何估计相机位姿？

而考虑一般情况，对于那些不是在同一平面上的点，如何估计相机位姿。

PnP原理示意如上图所示。简单来说，我们通过人工匹配或算法找到了2D(Image Feature)-3D(Point Cloud)的匹配点对。


二维特征点为 $x_1$，三维空间点位 $X^1$，将它们各自增加一维写成齐次。假设它们通过一个3×4的矩阵P(Projection Matrix)变换，λ表示从相机光心到真实三维点的距离。与单应矩阵H类似的，矩阵P中包含了内参K以及外参R、t，只要我们求得了P并且知道K，就可以恢复出R、t。这里采用与求解H类似的技巧，对齐次表示的 $x_1$向量(3×1)做叉乘，这样等式左边即为0，右边是其对应的反对称矩阵。通过一系列变换，使得未知量P在右边，已知量在左边作为参数，变成Ax=0形式。

PnP方程构建与求解。每对点提供2个独立的约束，P有12个元素，因此至少需要6对点(提供12个约束)才可以求解。在6对点时，稀疏矩阵大小为18×12。此方程可以使用最小二乘求解，对A进行SVD分解，数值解即是V的最右边一列。前4个元素为P的第一行，然后是P的第二行、第三行。

根据求得的P分解得到R、t。与H类似的，将K-1左乘数值求解得到的P’即可得到不包含内参的P。P’的前三列并不是最终我们想要的旋转矩阵R，我们还需要对前三列进行SVD分解，将分解的U和VT相乘，得到的这个矩阵才是我们想要的旋转R，这样做的目的是矩阵满足正交性。对于平移t，我们将P’的第四列除以原始P’矩阵前三列SVD分解的最大奇异值，即可以得到真实平移量。

需要简单说明的是，其实对于PnP的2D-3D位姿估计，其中的3D点并不仅仅局限于是点云，而可以是其它任何已知的真实世界中的3D点，例如GPS测量得到的X、Y、Z都是可以的。

3.0 Pose from 3D Point Correspondences or the Procrustes Problem：(3D-3D)给定一组对应的3D点对，找到它们之间的缩放、旋转以及平移关系。

当我在两边都有3D信息的时候 我们如何计算相机位置 在世界坐标系和相机坐标系中 , 在数学中这都被称作普罗克问题 .

简而言之就是给定一组对应的3D点对，找到它们之间的缩放、旋转以及平移关系。这种变换称为相似变换(similitude transformation)。在实际的刚体变换中，缩放s可以忽略，因此重点关注旋转与平移。这种3D-3D的变换可以用于如三维点云的对齐与配准。

采用RGB-D深度相机

因此问题就转化为了已知一些三维点对计算R、t。当然第一个问题就是我们需要最少多少个点就可以计算。

我们不妨从四个点开始，A2-A1为向量A21，A3-A1为向量A31。那么让向量A21与向量A31做叉乘，就可以得到方向满足右手定则且垂直于A21、A31组成平面的向量，不妨叫做A’。而得到这个向量后，再将其与向量A21再做叉乘，即可得到图中虚线所示的向量，不妨叫做A_。这个虚线向量与A21和A’都垂直，而A’又垂直于A21，所以它们构成了一个三维基底(坐标系)。

同理我们对B也可以得到这样一个基底。这样我们可以唯一地计算这两个坐标系之间的旋转。因此，只需要三对点即可以求解这个问题。

平移等于A-RB

但问题是，对于大多数情况下，实际匹配的点对是远远大于三对点的。这种情况我们将这个问题转化为一个优化问题，寻找变换(R、t)使误差总和最小。而t其实是可以通过计算A和旋转后的B的质心的差异得到的，简单来说就是计算A中所有点的平均值与旋转后B中所有点的平均值的差异。因此就可以对上式进行简化，得到A-RB。A、B均为减去均值后的坐标。

旋转R求解

采用矩阵论中的知识，形如这种的式子叫做Frobenius范数。式子中的前两项与R、t无关为常数，因此我们的优化目标就变成了最大化上式中的后两项。最终得到的R即为上图最后式子所示。

4.0 Pose from Projective Transformations利用单应矩阵H来实现投影变换求解相机的位姿。

由于这些特征都在地面(Zw=0)上，所以我们可以认为这些点的真实坐标的Z分量都为0，这样根据矩阵乘法，变换关系就可以简化。而且如果对前面介绍的单应矩阵有印象的话，这其实就是单应矩阵H。还是再强调一下H中的r1、r2分别表示地面上的x、y轴在相机坐标系下的表达(或者换句话说是世界坐标系的x、y轴相对于相机坐标系的旋转)。

我们有一个世界坐标矩阵，是在地上的。 其中有个z轴是面朝上的， 而X和Y是在平面上的，正如我们在几何学中学习的一样。 紧接着我们就有一个相机坐标系统，以c代表相机作为下标。

当所有东西都在这个地表平面时， 我们可以假设这些所有的点的坐标的Z值等于0。 这个假设有怎样的影响呢？

让我们来看一下投影变换， 尤其是那个我们已经在相机模型中学习过的从一个协调的坐标系 变换到一个相机坐标系的过程。

我们已知正交坐标系来源于世界坐标系统中的X,Y以及W。 我们已知选转变化r1，r2，r3以及平移变化T。 固有变量包括焦距，以及矩阵K 我们还有像素u，v，w 我们用θ， 这样我们可以确保左边是右边的倍数。 并且他们不是完全相等的。

现在如果世界中的z坐标等于0。 我们发现我们可以消除R3。

单应矩阵H

任何一个可逆变换都可以看作是一个投影变换。要看一个变换是不是可逆，可以检查变换矩阵对应的行列式的值是否为0。对于这里的情况，内参矩阵K的行列式值为f平方，因此当H的行列式不为0时，可逆。

现在我们将奇异值分解看做一个黑匣子。 你可以在Matlab中称其为一个方程。

U和V都是酉矩阵，即满足UTU=I,VTV=I。Σ是一个m×n的矩阵，除了主对角线上的元素以外全为0，主对角线上的每个元素都称为奇异值，V是一个n×n的矩阵。

使用投影变换，机器人相对于平面图案的姿势

5.0 Pose from Point Correspondences or the Perspective N Point Problem（PnP)(3D-2D)

• Pose from Point Correspondences

如图所示，如果利用罗盘等测得了船相对于两座灯塔之间的夹角，如何估计船所在的位置。可以发现，此时是无法精确确定船的位置的，船在圆上的任意一点运动夹角都相同。而对于3D情况，不确定会变成一个曲面。

我们需要第三点：P3P的透视三点问题或摄影测量中的切除问题：We need a 3rd point: The Perspective 3-Point problem of P3P Or in Photogrammetry the Resection Problem:

P3P可以利用余弦定理求解。

PnP还可以用直接解法求解。

将上面矩阵形式的式子“简单粗暴”地拆解后可以得到它们相互之间的表达式，由于刚体变换的未知数是6，所以至少需要3对点进行求解。这其实就是摄影测量里说的共线方程。这是一个非线性方程，因此需要迭代求解。