1.Epipolar Geometry
(1)对极几何基本原理
对极几何成立的条件就是两张或多张影像观测到同一场景(物体)。
要想计算对极几何，需要空间中至少8对3D对应点对。注意是三维点坐标而不是像素平面内的二维坐标。
在上图中，以Bob的第一人称坐标系(相机坐标系)为世界坐标系，x轴向右，y轴向下，z轴指向前方。这样对Bob而言，就有一个非常简单的相机投影矩阵P1。对于Mike而言，他的相机投影矩阵就是旋转R、平移t还有内参矩阵K。
如上图所示，Bob用手指向了建筑物的角点X1(在Bob的坐标系下度量)，Mike用手指向Bob，在他们之间形成了一个向量t，这个t是在Mike的坐标系下衡量的。
与此同时，Mike也可以指向Bob指向的点，这个点在Mike的坐标系下为X2。可以看到X1和X2是同一个点，只是在不同视角下的坐标。
因此，在上图中就有三条线：第一条是Bob和Mike之间的三维平移向量t(在Mike的坐标系下度量)；第二条是X2，由Mike指向建筑物的角点(在Mike坐标系下度量)；第三条是由Bob指向同一个角点，它在Mike的坐标系下定义为RX1或者X2-t。这样三条线可以组成一个平面，这个平面的法向量也很显而易见，t和X2作叉乘，即可得到该平面的法向量。当然，以“矩阵”的角度看向量时，把t转成对应的反对称矩阵与X2相乘即可。这样的话，X2-t这个向量肯定垂直于该平面的法向量。把它与平面法向量相乘结果为0，这样就可以得到一个等式(左上角所示)。对其进行整理化简，就可以写成第三行的形式。对于中间这部分，记为本质矩阵E。
可以看到，E是由t和R做叉乘得到的，所以这个简单的3×3矩阵包含了两个相机之间的相对旋转与平移。而且从上式也可以看出来它也相对好求，只需要一些对应点对即可。再温馨提示下，注意X1是Bob坐标系下的3维点，X2是Mike坐标系下的3维点。假如获得了多对对应点，就可以能求出E，既然E是由t和R叉乘得到的，所以得到E了以后就可以用一些数学方法分解得到R和t。
刚刚介绍了，Bob和Mike之间的三个向量形成了一个平面，这个平面与Mike的像平面相交，得到的交线就是极线(Epipolar Line)。而当他们同时指向另一个角点的时候，会发现这个平面以及它与Mike像平面的交线也跟着变化了。不同的点对应着不同的平面和极线，平面的方向随着我们指向的3D点而不断变化。值得注意的是在Mike像平面中的所有极线都会交于一点，这一点便成为极点(Epipole)。之所以出现这样的情况也是很容易理解的：因为不管这些平面如何变化，构成这个平面的向量之一t是不变的，t与Mike像平面的交点(如果像面足够大的话)也就是极点。
如上图所示，在Bob像平面中的x1可以写作是Bob相机坐标系中的X1的λ1倍。理解这句话需要用到以前学过的齐次坐标的知识，在齐次坐标中，所有的点都是用向量表示的。同理在Mike像平面中的x2可以写作是Mike相机坐标系中的X2的λ2倍，而且向量x2与该平面的法向量垂直。
同时我们可以合并EX1，E和X1的乘积是一个3维向量，这个向量可以看作是齐次坐标系下的一条直线。首先温习一下齐次坐标系下的直线表示：对于齐次坐标系下的两个点A和B，经过A、B的直线就等于从相机光心指向A形成的向量Va、指向B形成的向量Vb的叉积。换句话说就是直线AB就是Va、Vb构成平面的法向量。有了这个知识以后再来看EX1，把E写开就是：t×R·X1。先看R·X1它其实就是表示将Bob相机坐标系下的坐标旋转到了Mike的相机坐标系下，而且还在由t、X1、X2构成的平面内，这样的话t再与它做叉乘，得到的就是这个平面的法向量。而X2本身就是在这个平面里面的，所以X2再和这个法向量相乘结果为0。λ2X2也自然与EX1叉乘为0，所以说Mike像平面中的这一点x2经过EX1这条线。EX1就是经过X2点的在Mike像平面里的极线。
同理反向推导一下就可以得到X1在Bob像平面中的极线为X2^TE。
而对于Bob像平面中的极点和Mike像平面中的极点可分别按上图中的公式求解，形成一个方程组，用最小二乘做SVD分解即可求解得到e1或e2向量，也就是齐次表示的极点坐标。极点可以看作是两个相机光心之间的向量，因此两个相机一定都会在这条线上(只能知道在这条线上或者说方向上，但不知道具体在哪里)。
(2)本质矩阵与基础矩阵计算
仔细观察上面的公式可以发现都是在相机坐标系下研究的。X1和X2我们也一直都说是Bob和Mike各自相机坐标系下的三维坐标。但实际上更一般的情况是我们直接拿到的或者匹配得到的是两张影像上的像素坐标，是在影像(像素)坐标系下讨论问题的。所以我们不妨把x1=KX1、x2=KX2，从相机坐标系到像素坐标系的转换放进来，合并写成如下图所示。
这样，我们就把中间的既包含E又包含K的这一部分起个名字叫做基础矩阵F，它概括了两个相机之间的所有关系(成像、旋转、平移)。可以看到，由于我们把相机到像素坐标系的转换放到基础矩阵F里了，所以基础矩阵“沟通”的就是两张影像像素坐标之间的关系。这就非常好，正是我们想要的。而且如果知道内参矩阵K，由F恢复E也是容易的。
求解基础矩阵需要8对点，每一对点都可以提供一个方程。把这个方程中的f元素提取出来，写成Ax=0的形式如下。
对于这种齐次方程，可以按照下面的方法求解。也就是之前最常用的SVD分解然后取V的最后一列。
这里需要注意的是获得的9×1的解向量中F矩阵中各元素的顺序。前三个为F矩阵的第一列，然后是第二列、第三列。这点需要注意。这和之前求解单应矩阵不同，求解单应矩阵时解的顺序是h11、h12、h13，前三个为H矩阵的第一行，以此类推。需要注意元素顺序，因为这涉及到是否需要转置的问题。当然，最后别忘了数值解法得到的F不一定满足F性质(秩等于2)的，所以需要用SVD“清洗”一下结果，得到满足条件的最逼近于求解结果的F矩阵。简单来说就是对得到的F矩阵再进行一次SVD分解，让分解得到的D的最后一个元素为0，重新相乘得到的F就是符合“秩约束”的最逼近的结果。这里之所以说是“逼近”或者说“近似”是因为我们强行让D的最后一项为0,相当于我们忽略了那一维所代表的矩阵信息。利用SVD恢复了一个和求解得到的F相似的矩阵。这一块就是SVD分解的用途了，在前面的笔记中有提到。我们强行让D的最后一项为0是否合理呢？答案是合理的。因为一般而言，求解出来的D的最后一个元素都会接近0，让它为0不会损失什么精度。另外需要注意的是，我们这里是使用最小二乘求解的F，还记得之前提到和演示过的最小二乘对于Outliers特别敏感，我们无法100%保证通过匹配算法得到的匹配点是精确的，所以用于求解F的点对很可能有Outliers。对于这种情况之前也提到过，就是是用RANSAC进行过滤。切记不要忘了RANSAC。完整求解流程如下。
一个计算基础矩阵F的例子。利用SVD数值求解得到F之后，再对F进行SVD分解，使D的最后一项为0，重新构建，得到最终的F。
在得到基础矩阵F之后就可以求极线了。这样求得的是一个齐次坐标表示的三维向量，然后使其前两项的模为1，就可以得到方向cos和sin了。
此外，还可以求解两张影像上的极点。求解得到的是一个齐次坐标系下的三维向量，要想获得在像平面内的坐标，需要使向量的最后一维为1，这样得到的就是像平面内的坐标了。
(3)由本质矩阵或基础矩阵恢复旋转与平移
前面两小节介绍了本质矩阵的由来以及在给定对应点对下的解法。但我们一开始的目的是想恢复两个相机间的相对位置关系(旋转与平移)，所以这一小节研究对本质矩阵的分解。下面两图显示了E和K的具体换算公式。
也就是说，假如我们根据对应像素坐标对求解得到了基础矩阵F，如果又已知内参矩阵K，就可以由上面的公式求解得到本质矩阵E。而根据前面E的定义它等于t×R。
其实t表示影像2中的极点(齐次表示)。这个需要从极点的定义进行理解，极点就表示另一个相机的光心在当前影像中的位置。对于Bob的坐标系原点，以齐次坐标形式写即为(0,0,0,1)。而将它与R和t构成的投影矩阵P2相乘，就可以得到Bob坐标系的原点在Mike坐标系下的坐标，而这个坐标就是Mike坐标系中的极点。
另一方面，根据前面的知识可以知道，影像2中的极点与E相乘结果为0(e2^TE=0)。所以这样便可以得到一个关于t的方程。我们可以对E进行SVD分解，得到的U的第三列即是E的零空间，所以t=u3。但由于齐次坐标的尺度不确定性，乘以任意非零常数(可正可负)都成立，所以最终t有u3和-u3两种情况。所以上面说了一大堆，简单来说就是平移t等于对E进行SVD分解，分解结果U的最右边一列，并且有正负两种情况。
因为前面已经得到t，它就是U矩阵的第三列，所以将t的反对称矩阵改写成图中绿色部分所示的内容。这样就用U矩阵表达了t的反对称矩阵。至于中间的这个矩阵怎么来的，课程里有讲解，但没太听懂。
对于旋转矩阵R，我们也可以认为包含三个部分：U、Y、VT，这里Y是未知量，其余都是已知量。这样假设是便于后续计算方便。
这样的话，UT和U相乘就消去成单位阵了。
于是式子就变成上面的这种形式了。按照对应项相等的原则，可以发现等式两边粉色部分应该相等。这样就构造了一个可以用于求解Y的约束。
最后可以求解得到Y有两种情况，如上图所示。
可以看到，t有两种情况，R有两种情况，所以共有4种可能性。这里需要注意的是，如果恢复出来的R的行列式为-1，则不满足旋转矩阵的性质了，这时就对t和R同时添加负号。
最后总结一下计算过程。给定两张影像，获得对应的坐标点对，先计算基础矩阵F，然后结合相机内参矩阵K，恢复本质矩阵E。对E进行SVD分解，平移t就等于U的最右边一列(正负两种情况)。旋转R就等于Y或YT与VT的乘积，Y就是上面提到的矩阵。如果恢复出来的R行列式为-1，对R和t同时取负号。
对于多种情况的过滤，可以通过三角化实现。简单来说就是根据得到的R、t对影像中的点进行三角化，看三角化后的三维坐标在第一帧的坐标系下的Z分量是否为正，如果为负则说明是错的。实际操作中由于误匹配噪声等影响，不可能所有点都为正，所以是判断4种情况中哪个对应获得的Z分量为正的坐标点数量最多就认为哪个估计是准确的。状态筛选的流程如下。
最后需要提醒的是，求解的相机位姿是相对于第一帧的位姿，所以说如果想获得在世界坐标系下的真实位姿还需要再根据第一帧的真实姿态进行转换。
(4)三角化
点的三角化可以理解为通过两张或两张以上影像(影像间彼此的相对旋转与平移已知)上二维坐标的对应关系恢复点的三维坐标的办法。它实际上也是一个最小二乘问题。
如上图所示3D点X通过投影矩阵P1变换到了像素坐标x1，相差尺度λ，表示从光心到该3D点的距离。然后对等式两边进行操作，同乘以x1齐次坐标的叉乘，等式就为0(向量与其自身的叉乘为0向量)。之所以要这样做是因为我们希望求解的是X，希望可以构建Ax=0形式的方程进而求解。而事实上这也是可以的，紫色部分即为系数矩阵A。这是通过一个视角获得的方程。
对于多个视角，可以获得多个方程如上图所示。每一个视角都可以一共一个这样的方程，多个方程叠加在一起一共构成一个系数矩阵A。在实际操作中，根据点的像素坐标以及投影矩阵就可以构建出系数矩阵了。这就是一个熟悉的最小二乘解方程问题，利用SVD对系数矩阵分解取V的最右边一列即可得到结果。此外需要注意的是系数矩阵的秩为2，所以至少需要两个方程才可以求解。
一个三角化的例子。
如上图所示，以第一张影像的坐标系为参考坐标系(世界坐标系)，两个相机之间只发生了平移C，P1、P2为各自的投影矩阵。
根据上面的介绍，已知像素坐标和投影矩阵就可以构建系数矩阵A了。进而可以求解出该点三维坐标。准确的说是先求出了四维的齐次坐标，然后对最后一维归一化，归一化后向量的前三个分量即是要求的结果。
三角化计算3D点的流程总结如下。
2.RANSAC Part 2
这一部分主要介绍RANSAC在估计基础矩阵中的实际应用实例，RANSAC本质上就是Majority Voting的思想。如下图所示是没有经过筛选的匹配点对。
而求解基础矩阵需要至少8对点，所以我们可以随机挑选8对点，如下所示，并统计内点个数。在实际中RANSAC一般要运行很多次，才会得到比较好的结果。之前说的那些公式是得到正确结果的最小迭代次数，一般而言比它更大一些会更好一些。
这里面涉及到的一个问题就是如何评价一个点是不是内点，误差度量指标是什么。简单来说就是在第一张影像中给定一个点，利用基础矩阵F可以算出其在第二张影像中的极线，理论上来说第二张影像中的对应点应该在这条极线上，所以检查第二张影像中的对应点是否在极线或指定阈值范围(一般而言阈值取1到2个像素)内即可判断是否是内点。当然按照“重投影误差”的思路也是可以的，有了F和第一张影像中的像素坐标，解算第二张影像中的对应坐标，并计算它与提取的对应点之间的欧氏距离，小于阈值即认为是内点。
以后用到最小二乘就要想到RANSAC，它作为一种安全机制，保证了在有噪声的数据中可以提取到正确的模型。
3.Nonlinear Least Squares Problem
(1)线性最小二乘简单回顾
之前学过的线性最小二乘问题主要可以分为齐次和非齐次两类，都可以很高效地求解。
线性最小二乘也有一些很好的性质：
例如有全局唯一解、可以有解析解 (无需迭代)、可以用SVD高效求解、无需迭代初始化等。
但现实永远都不可能那么美好。事实上在实际中大部分变量之间的关系都不是线性的，需要我们用更复杂的手段去解决。
(2)非线性最小二乘的两个例子
在之前的PnP问题中，我们通过将投影矩阵P“线性化”变成一个向量的方式从而构建了Ax=b的形式进而用最小二乘配合SVD求解。但这样的问题是，求解出来的P并不满足R的内在约束，如正交性，因为P=K[R t]，所以说P并不是一个任意的向量。但当我们将P拆分成一个长长的向量的时候，就已经损失了这种约束。所以，如果用非线性最小二乘就可以在满足这种约束的情况下找到最优解。
第二个例子是三角化(给定影像间的位姿以及目标点的影像坐标，计算对应的三维坐标)。这在之前也说过了，可以通过构造最小二乘问题利用SVD进行方便地求解。但这里存在问题。
问题就在于在之前提到过，最小二乘方法对于噪声非常敏感，上面的方法可能会存在很大的误差。只有当没有噪声的理想情况时，上面的方法才会得到比较准确的结果。但事实上由于镜头畸变、误匹配等因素误差不可避免，这就会导致这些光线最终不会准确交于一点，如上图所示。Ax不会准确等于0，而是其它的某些数字。如果我们按照线性的方法算了，把最终算得的结果重新重投影到各个影像中，会发现和真实的像素点存在偏差。
所以事实上，我们想要获得的三维点是一个尽可能使它重投影到各影像中误差最小(对应图中红色和蓝色箭头重合)的点。也就是说我们希望在像素空间中最小化重投影误差而不是在三维空间中。所以我们可以按如下图所示进行操作。
(u,v)是我们提取的特征点，(urepo,vrepo)是重投影得到的点。重投影得到的点按上图公式计算。P是投影矩阵，X~为估计的三维点坐标，PX~则是得到的齐次表示的像面坐标，同时为了获得像素坐标，除以其自身的第三维使其为1，即可得到像素坐标。公式是相对好理解的。正是这个除法造成了非线性的表达。
在获得了(u,v)和(urepo,vrepo)之后，就可以很自然的写出它们之间的误差(以欧氏距离度量)如上图所示。而我们有多个影像，每一个影像都有一个重投影误差，我们最终要最小化的就是重投影误差之和。而很明显，这是一个包含除法与平方的非线性表达，对应非线性优化问题。
(3)非线性最小二乘的求解
对于非线性最小二乘，一般优化的是f(x)-b的平方这个式子，等式右边按照平方展开法则展开，由于bTb与自变量x无关，为常量，所以在优化时被忽略，所以最终优化的就是第三行的式子。不断改变x的值使得该式的值最小，需要注意的是这里的自变量x是一个向量而不是一个数字。这里的一个例子是测得了当前点距离5个灯塔的距离，求解该点的最合适位置使其满足这些观测。
这里误差E的定义是STS，其中S是某点到某个灯塔的距离与测量值的差异，是一个包含5个元素的列向量。E为一个数。这样对每一个位置逐个遍历计算E，就可以得到上图中彩色显示的结果。但在实际中不可能遍历所有结果去寻找最优解的。所以需要导数，要解决两个问题：什么时候是最值？如果不是最值应该如何走更加接近最值？
对于第一个问题，其实比较容易回答，对代价函数求导，导数为0的点即为局部极值或全局最优解。对于矩阵函数而言，就是求对应的雅可比矩阵。这里f(x)是一个矩阵函数。自变量x是我们的观测量(x,y)。而约束条件一共有5个，所以雅可比矩阵的大小即为5×2。更一般情况雅可比矩阵的每列表示一个自变量，而每行表示一个约束(观测)。这点一定要理解。
对于第二个问题，其实又可以细分为两个问题：该向哪个方向走？走多远？它们可以反映在Δx的大小与正负上。我们可以对代价函数的导数在x这点进行泰勒展开，将展开结果带回原式，化简就可以得到上图中最后一个式子。这个式子是一个最小二乘问题，可以进行求解，进而获得Δx。
获得Δx以后就可以进行移动了。得到新的Xk+1，然后重复上述步骤。
最后简单总结如上图。f(x,y)是一个拥有两个自变量的非线性矩阵函数，其本身是一个5×1的向量。每输入一对(x,y)就可以得到一个5×1的向量，每个元素表示到每个灯塔的距离。b也是一个5×1的向量，表示测得的到各灯塔的距离。我们需要优化的就是f(x,y)与b的差值。同时f(x)对自变量x的导数是一个矩阵，这个矩阵称为雅可比矩阵。矩阵的列数表示自变量元素个数(维数)，矩阵的行数表示有多少个约束。当然这里还需注意的是，由于是迭代方法，所以不同的初值会对结果造成影响，如上图左边所示。
本文作者原创,未经许可不得转载,谢谢配合
