这一篇是单应矩阵（Homography Matrix）分解得到 R ，t的笔记。ORB-SLAM2中用Faugeras(1988)的方法实现了单应性矩阵分解的，而在OpenCV中decomposeHomographyMat函数是用INRIA(2007)的方法实现的，并且源码里面也有Zhang(1996)的实现。这篇主要参考了泡泡机器人ORB-SLAM2注释和Faugeras 论文，介绍Faugeras SVD-based decomposition的推导。
首先，设空间上的点 X=(XYZ)T 和点在图像平面的坐标 x=(xy1)T ，它们有如下关系：
Xx=Yy=Z(1) 令第一个相机坐标系为世界坐标，投影模型为 P1=K1[I|0] ， P2=K2[R|t] 。设3D点所在的世界平面的方程为 aX+bY+cZ=d ，平面单位法向量 n=( abc)T ，对在平面上的点 X ，平面方程可表示为 1dnTX=1(2) 对于两个相机坐标系的点有如下关系 X2=RX1+t(3) 由于点 X1 满足直线方程 (2) ，有 1dnTX1=1 ，则把该式乘到 (3) 式 t 的右侧得 X2=(R+1dtnT)X1(4) 根据单应性变换，对图像齐次坐标 x=(xy1)T 和齐次单应矩阵 H ，两个图像坐标满足αhx2=Hx1，根据投影模型 x1=K1X1 ， x2=K2X2 ，我们有 X2=K−12HK1X1(5) 对比 (4) 和 (5) 我们有 A=dR+tnT=dP−12HP1(6) 对 A 进行奇异值分解得到A=UΛVT（ UTU=VTV=I ）， Λ=diag(d1,d2,d3) （ d1≥d2≥d3 ），则有 Λ=UTAV=dUTRV+(UTt)(VTn)T 有 s=detUdetV ， s2=1 ，令 ⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪R′=sUTRVt′=UTtn′=VTnd′=sds=detUdetV 我们有 Λ=⎡⎣⎢d1000d2000d3⎤⎦⎥=d′R′+t′n′T(7) 取基底 e=(100)T ， e=(010)T ， e=(001)T ，令 n′=(x1x2x3)T=x1e1+x2e2+x3e3 ，根据 (7) 式可得 [d1e1d2e2d3e3]=[d′R′e1d′R′e2d′R′e3]+[t′x1t′x2t′x3](8) 即有三个等式 d1e1=d′R′e1+t′x1d2e2=d′R′e2+t′x2d3e3=d′R′e3+t′x3(8.1)(8.2)(8.3)注意到 n 是单位法向量， V 是旋转矩阵，则n′也是单位法向量，则有 Σ3i=1x2i=1 。对 (8) 的三个式子消去 t′ 得
d′R′(x2e1−x1e2)=d1x2e1−d2x1e2d′R′(x3e2−x2e3)=d2x3e2−d3x2e3d′R′(x1e3−x3e1)=d3x1e3−d1x3e1(9.1)(9.2)(9.3) 这里的 R′ 也是旋转矩阵，满足 R′TR′=I 。也就是 R′ 具有保范性，也就是 ∥R′X∥=∥X∥ ，对 (9) 式两边取二范数可得 ⎧⎩⎨⎪⎪(d′2−d22)x21+(d′2−d21)x22=0(d′2−d23)x22+(d′2−d22)x23=0(d′2−d21)x23+(d′2−d23)x21=0(10) (10) 式是一个关于 x21 、 x22 和 x23 的一个齐次线性方程，其必然有非零解，那么系数矩阵的秩小于 3 ，其行列式必为0，也就有 (d′2−d21)(d′2−d22)(d′2−d23)=0(11) 根据 A 奇异值的大小顺序d1≥d2≥d3，从而有三种情况 d1≠d2≠d3 d1=d2≠d3 或者 d1≠d2=d3 d1=d2=d3对于上述3种情况，都有 d′=±d2 ，而解 d′=±d1 和 d′=±d3 都是不可能的。对于第一种我们考虑如果 d′=d1 ，则根据 (10) 式有
x1=0(d′2−d23)x22+(d′2−d22)x23=0 由于 d1≥d2≥d3 ，但是根据上式得到的是 x2=x3=0 ，那么不满足 n′ 是单位阵的条件 Σ3i=1x2i=1 ，则这样的情况不存在。如果 d1≠d3 ，根据式 (10) 以及 Σ3i=1x2i=1 ，有
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪x1=ε1d21−d22d21−d23−−−−−√x2=0x3=ε3d22−d23d11−d23−−−−−√ε1,ε3=±1(12) 接下来分成两种情况进行讨论由于 x2=0 ，代入式 (8.2) 得 e2=R′e2 ，所以 R′ 是沿着轴 e2 旋转，那么有
R′=⎛⎝⎜cosθ0sinθ010−sinθ0cosθ⎞⎠⎟(13) 把 R′ 代回式 (9.3) ，有 ⎛⎝⎜cosθ0sinθ010−sinθ0cosθ⎞⎠⎟⎛⎝⎜−x30x1⎞⎠⎟=⎛⎝⎜−d1x30d3x1⎞⎠⎟ 结合式 (12) 可以解得 ⎧⎩⎨⎪⎪⎪⎪⎪⎪sinθ=d1−d3d2x1x3=ε1ε3(d21−d22)(d22−d23)√(d1−d3)d2cosθ=d1x23−d3x21d2=d22+d1d3(d1+d3)d2(14) 由于 n′Tn′=1 ， (8) 式两边同乘以 n′ 然后移位可以得到 t′=⎛⎝⎜d1000d2000d3⎞⎠⎟⎛⎝⎜x10x3⎞⎠⎟−d2⎛⎝⎜cosθ0sinθ010−sinθ0cosθ⎞⎠⎟⎛⎝⎜x10x3⎞⎠⎟(15) 把式 (12)∼(13) 代回 (15) 有 t′=(d1−d3)⎛⎝⎜x10x3⎞⎠⎟(16)看式子 (12) ，这时有 x1=x2=0 ， x3=±1 ，这时 n′=(00±1)T ，解为
{R′=It′=(d3−d1)n′(17)这时 x1 ， x2 ， x3 未定义，根据式子 (8) ， (9) 有
{R′=It′=0(18)这时 d′=−d2 ，代入式 (8.2) 得 −e2=R′e2 ，这时的 R′ 可以表示为
R′=⎛⎝⎜cosθ0sinθ0−10sinθ0−cosθ⎞⎠⎟(19)同样把 R′ 带回 (9.3) ，得
⎛⎝⎜cosθ0sinθ0−10sinθ0−cosθ⎞⎠⎟⎛⎝⎜−x30x1⎞⎠⎟=⎛⎝⎜−d1x30d3x1⎞⎠⎟ 结合式 (12) 可以解得 ⎧⎩⎨⎪⎪⎪⎪⎪⎪sinθ=d1+d3d2x1x3=ε1ε3(d21−d22)(d22−d23)√(d1−d3)d2cosθ=d3x21−d1x23d2=d1d3−d22(d1−d3)d2(20) 把式 (12) ， (19) 和 (20) 代回 (8) 有 t′=⎛⎝⎜d1000d2000d3⎞⎠⎟⎛⎝⎜x10x3⎞⎠⎟+d2⎛⎝⎜cosθ0sinθ0−10sinθ0−cosθ⎞⎠⎟⎛⎝⎜x10x3⎞⎠⎟=(d1+d3)⎛⎝⎜x10x3⎞⎠⎟(21)同样结合式子 (12) ，这时有 x1=x2=0 ， x3=±1 ，这时的解为
⎧⎩⎨⎪⎪⎪⎪⎪⎪R′=⎛⎝⎜−1000−10001⎞⎠⎟t′=(d3+d1)n′(22)根据公式 (7) 有 −d′I=d′R′+t′n′T ，也就有，对于与 n′ 垂直（在平面内）的向量 x ，有 R′x=−x ，根据householder变换得到：
{R′=−I+2n′n′Tt′=−2d′n′(23)上述的两大类情况，除了未定义的情况，2和3都是1的特例。由于我们不知道 d′ 的符号，所以一共有8个可能的解。考虑
n′=⎛⎝⎜⎜⎜⎜⎜⎜⎜⎜ε1d21−d22d21−d23−−−−−−−√0ε3d22−d23d11−d23−−−−−−−√⎞⎠⎟⎟⎟⎟⎟⎟⎟⎟(24) 其中 R′ 中的 sinθ 中有 ε1ε3 项， t′ 中存在 n′ ，所以对 ε1=±1 ， ε3±1 一共有四种解，对于 d′<0 和 d′>0 一共有 4×2 种情况。但是从物理的角度去考虑，只有 2 种情况是可能的解。考虑(6)式 A=dR+tnT=dP−12HP1 由于 A 是已经确定的，所以s=detUdetV是确定的，所以 d′=sd 也是确定的，因此 8 个解只剩下4个。对于解得的平面单位法向量 n 为 n1 ， −n1 ， n2 ， −n2 ，由于 nTX=d ，根据 (1) 式，考虑 Z1>0 ，有 nTX1dZ1=nTx1d>0 所以只有其中两个 n 是满足条件的。在Faugeras论文中提到，当观测到的点不是全部靠近一个相机光心而远离另外一个相机光心，那么只有一个解满足所有的点。如果有 d′>0 ，对于上述两个解 (n′1,t′1) 和 (n′2,t′2) ，这两个解有如下关系，也就是两个解是相同的。
{t′2=(d1−d3)n′1n′2=t′1d1−d3 而 ORB-SLAM2的代码，是在求出所有的8个解之后，对每一个解进行分析，检测点是不是都在两个相机的前方，并且统计重投影误差较小的点的个数，找出8个解中统计得到点数最多的的那个解作为最终的解。