SLAM笔记（四）运动恢复结构的数学基础(本征矩阵F，单应矩阵H，八点法与五点法，四点法)

1. 间接法前提假设

对于结构与运动或视觉三维重建中，通常假设已经通过特征匹配等方法获取了匹配好的点对。这种方法先求出匹配点对再获取结构和运动信息的方法称作间接法。间接法最重要的三个假设是：
1.拥有一堆两帧之间的匹配点对。同时假设:
- a.匹配点不一定精确，即对于匹配点对 (An,An+1)，An+1可能并非An的真实匹配 点，而是在它周围；
- b.匹配关系不一定正确。
2.场景是静态的，即环境中不存在运动的物体
3.知道相机内参数且内参数恒定不变。
此处的匹配点，可以是2D-2D(单目相机初始化时)，2D-3D配对点（单目相机运行过程中，已经算出了3D的地图点，又来了2D的图形），或是3D-3D的匹配点（使用双目相机、RGBD相机或其他传感器，可以直接过间接获取匹配好的三维点对）。本文默认是2D-2D的配对点。

2.极线约束，本征矩阵

极限约束：
物理世界上的一个三维点P投影到两个成像面（实际上像面也常常处理成z=1的地方）上，点P与两个像面的光心形成一个对极平面，l1,l2分别为对极平面与像平面相交的极线。
极限约束是指对第一个成像面上的点p1，它在第二个平面上的对应点p2总存在于对应的极线l2上。
用x表示像平面坐标的话，K为内参数矩阵，P为相机坐标系三维点；如果已知内参数K，则可以去掉K的影响，即令x=K−1p，得到

λ1x1=P，λ2x2=RP+T

即：

λ2x2=λ1Rx1+T

(2-1)
此处的x1,x2可以看做是z=1平面上的点。
极线约束（Epipolar constraint，essential constraint，bilinear constraint）:
对于两帧之间的归一化坐标x1,x2，

xT2T^Rx1=0，（2−1）

其推导过程如下：
1。由于λx=X(相机坐标系下世界坐标)，则可得：

R，T分别为旋转和平移矩阵。
2.左右分别左乘T^，即得到

3.左乘x2，除掉λ影响:（将此缩放因子放入E中，此缩放因子在4部分会介绍），将上式左右两边投影到x2上，即左乘上xT2，左侧即为0（因为T×x2垂直于x2，因此左乘x2即为0），得极线约束：

xT2T^Rx1=0

注：
上述2，3过程中，T^，x2都是不可逆的，因此这两个投影都是不可逆投影(与下文的可逆投影–单应性不同)；某种意义上这两个过程将强约束转化成弱约束了，但获得的好处是将二维点直接联系起来，不用考虑三维点。

极线约束的几何意义：

x1,x2分别为从光心发出的向量。由于T为光心之间连线的向量，三个向量共面，所以该triple product为0。
对于式xT2Ex1=0，如果令l=Ex1
(Ex1结果为{a,b,c}，对应于右侧像平面坐标上的直线ax+bx+c=0)，则存在：xT2l=0，即x2存在于右侧极线l上，
本征矩阵（Essential Matrix）:E=T^R，则：

xT2Ex1=0

（2-2）
E组成的空间称为本征空间：

一个非零矩阵当且仅当（充要条件）
svd 分解中Σ是：

diag{a,a,0},a>0

时，这个矩阵是本征矩阵。
（思考：为什么特征空间需要这样定义）
此时可由E求得R和T：

因此视觉SFM（structure of motion）问题中恢复分两步走：
- 1..Recover the Essential Matrix frome the epipolar constraints
- 2.Extract the R,T if we know the Essential Matrix:E=UσV
从一系列极线约束中恢复的矩阵E一般是不满足本征矩阵的条件的；怎么办，再将E投影到本征空间即可，具体做法是称为八点法。

3.运动信息重建（旋转和平移信息）

3.1 八点法（Eight-Point Algorithm）

3.1.1从点对中求解出矩阵E

先将矩阵E写作栈的形式：

同时，假设点的齐次向量x1,x2表示成如下形式：


则极线约束可换成两个向量乘积的形式：

xT2Ex1=aTES，（2−3）

对于n个点对：上式为：

χES=0,withχ=(a1,a2,a3,...,an)T，(2−4)

因此求解E变成求解方程(2-3），即χ的零空间E。为了得到唯一解（除了平凡解E=0），χ的秩应该是8。至少需要8个点对，即χ的列数为8，rank(χ)才可能为8。当然，假如点对中某些点相互之间是线性相关，比如3个或以上的点在同一直线上，或者4个或以上点在同一平面上，则秩并非点对个数，此时需要8个以上的点才可能满足要求。
由于无法知道E的正负（方向），因此结合每个E有两个可能的(R,T)组合，最终可能有四个可能的(R,T)

3.1.2将矩阵E投影到本征空间中

由于E往往不满足上部分2中特征空间的要求，因此需要将E投影到特征空间上，具体做法是：
对于E=Udiag{λ1,λ2,λ3}VT，投影到特征空间后为：
Ep=Udiag{σ,σ,0}VT,with  σ=（λ1+λ2)/2
（思考：Since in the reconstruction, E is only defined up to a scalar）
在三维重建中一般将Ep投影到归一化的特征空间上:即Ep=Udiag{σ,σ,0}VT除以σ，得到归一化的结果Ep=Udiag{1,1,0}VT。

总结：八点法（Longuet-Higgins ’81）的具体做法：
1.通过最小化||χEs=0||计算近似的本征矩阵E；
2.SVD分解E，并E投影到特征空间，并将结果归一化
3.从特征矩阵中恢复出R,T：

求出的四个(R,T)对的效果如下图所示：

能看出四个(R,T)解中只有一个解是有意义的，只有这个解才能使所有点的深度是正的。

3.2 五点法（Five-Point Algorighm）

矩阵E的自由度:
由于E由三个旋转，三个位移构成，一共六个自由度；但由于χE=0中对于E乘上一个数λ齐次式都成立，因此E实际自由度为5。
Kruppa 在 1913证明只需要5个匹配点对就能算出E，即五点法。
由于五点法引入了非线性约束，解起来麻烦，于是这儿就不详解了，只是大概表述如下：

χ(p1E1+p2E2+p3+E3+p4E4)=0

如果有额外的信息，比如运动是在同一平面运动或绕圆周运动，又比如有了惯性传感器可以知道其他位姿信息，则需要更少的点就可以求出本征矩阵。
continuous epipolar constraint:产生线速度和角速度而非位移T和旋转R。
* independently moving objects：*
(xT2E1x1)(xT2E2x1)=0(two independent motion)

3.3 四点法，单应性矩阵H

3.3.1 单应矩阵到底是个什么东西

单应映射就是线性映射关系
当我们从两幅image上看到真实世界中的同一个有限平面（如一个长方形）时，假设长方形在image1中形成的投影为四边形1，在image2中形成的投影为四边形2；假设长方形任何一个点在四边形1的对应的点x1，在四边形2中的对应点x2，这些二维点对(x1，x2) 有无数个，但可以用一个3x3的单应矩阵H来表示所有的对应关系：x2=Hx1。这不就和
从形式上看，这不就是二维点与二维点之间的映射关系吗？比如举一个最简单的例子，二维旋转矩阵就是一个简单的单应矩阵：

再来一个栗子：
单应矩阵可以将一个二维的正方形做如下变换（r代表旋转矩阵的元素，s表示缩放的标量）

3.3.2数学上的单应性矩阵H

明白了使用场景，我们再来看单应性（homography，在多视图几何中又称为projectivity（射影性），collineation(直射变换，共线性），projective transformation（投影变换））的定义：
的定义：

定义：
单应性是一个存在于空间P2 内的可逆映射h，h满足：三个点共线当且仅当映射后的三个点共线。
或者说：
P2−>P2是单应映射当且仅当存在一个非奇异的8维 3x3矩阵H，对于任何一个x∈P2,总能找到对应的点Hx∈P2。（这儿的P2是二维空间的齐次表达，可以当成是二维平面空间）
当然单应性不止局限于2x2平面，也可以有n维空间之间的单应映射：
Pn−>Pn是单应映射当且仅当存在一个非奇异的(n+1)2−1维度的(n+1)方阵H，对于任何一个x∈Pn,总能找到对应的点Hx∈Pn。
比如，在三维投影空间(P3)中，三维点之间的线性映射关系可以用4x4,15维度的H表示。
当一些点在同一个平面上时，设NT是平面的法向量，d为点到面的距离，则对于该平面上任一三维点X1：

上式右乘到下式右边：

其中H成为单应矩阵（homography matrix）：

如果令x=KX，x为像素坐标，则上式为：
K−1x2=(R+TNTd)K−1x1
则H=K(R+TNTd)K−1
(根据空间平面单位和位置不同而产生不同的H映射，实际运用中更多的是直接使用像素坐标而非物理化后的metric 坐标)
对x2=Hx1左乘一个xˆ

xˆ2Hx1=0

这是单应性的另一种形式。
注意这个式子与本征约束式子之间的差别：一个是x叉乘，一个是x转置。化简后本式实际上是一个线性方程组（0是一个3维向量），本征矩阵约束是一个线性方程（0是一个标量），这导致了H约束个数的翻倍：只用四个点对即可求解

单应性约束与极限约束：极线约束只是点与对应一条极线的关系，而单应性矩阵约束更强（从单应性矩阵可逆而本征矩阵缺秩也可以看出），是点到点的一一对应。
所以对于物理空间上的同平面

这之后计算H的方法和式（2-1）的处理方式相似，都是根据：

可以用直接线性法（DLT，Direct Linear Transformation）求解线性方程(参见附录1)

从H组成可以看出H取决于R,T和平面自身的参数。
算出H后，再根据H=R+1dTNT 可以求得各项其他参数: 旋转R,法向量N和T/d(注意的是T/d而非两幅图对应pose的平移T)，即当d很远时，即离平面很远的情况下，T可能不那么准确，因此全景拼接（d一般较大）时只做纯旋转，效果可能会更好。

此外，实际计算单应性时，由于H对噪声敏感，需要将所有点进行归一化处理成平均值为0，方差为固定（如sqrt（2）），随后根据像素点与像素点的对应关系求出H。

3.3.3单应性矩阵的特殊情况

当T=0，则H=R，即有X2=RX1。此时等价与d为无穷远；或者说，无法计算d，因为d为任何值都可以。
E与H的关系
将E=T^R,H=R+Tu相互代入：
E=T^H，HTE+ETH=0

3.4 基础矩阵（fundamental matrix）

当不知道内参数矩阵时，只有像素坐标，那悲催了，堆积约束为：

pT2K−1T×RK−1p1

K−1T×RK−1成为基础矩阵F。由于K不影响秩因此矩阵是基础矩阵的充要条件是它的秩为2。可以看出基础矩阵的给与的约束不强。
在SFM,SLAM中我们一般认为相机已经标定好，因此基础矩阵不常用。

4.结构信息重建（地图点三维信息）

求出R,T后，对于某点对，其 关系如下：

其中:
γ=||E||=||T^R||=||T^||=||T||=σ2，σ是特征矩阵E的SVD分解的特征值 是尺度因子。因为(2-2)中将尺度因子放入E中了。归一化的E行列式为1，即Σ={1,1,0}2
但λji无法求得。为此对左右乘以λj2^:

上式可以写作：

对与所有点对(xj1,xj2),j=1,2,3,...，则有：

λ⃗ 即是MTM的特征向量；
可求出所有点的实际三维点λj(x,y,1)T信息和尺度因子γ。
附录：
1直接线性法:multi-view geometry
2homograph:
homograph1
homograph2