从本质矩阵恢复相机矩阵

本质矩阵

本质矩阵（essential matrix ）是基本矩阵在归一化图像坐标下的一种特殊形式。

考虑相机矩阵P=K[R∣t]P=K[R|\mathbf t]P=K[R∣t]，点x=PX\mathbf x=P\mathbf Xx=PX是图像平面上的一个点，若已知相机内参KKK，那么点x^=K−1x\mathbf{\hat x}=K^{-1}\mathbf xx^=K−1x，有x^=[R∣t]X\mathbf{\hat x}=[R|\mathbf t]\mathbf Xx^=[R∣t]X，则称点x^\mathbf{\hat x}x^是在归一化坐标（normalized coordinates ）下的表示。点x^\mathbf{\hat x}x^可以被认为是空间点X\mathbf XX在内参矩阵为单位阵III的情况下的像，而K−1P=[R∣t]K^{-1}P=[R|\mathbf t]K−1P=[R∣t]称为归一化相机矩阵（normalized camera matrix ）。

对于一对对应点x↔x′\mathbf x\leftrightarrow\mathbf x'x↔x′基本矩阵定义为
x′Fx=0(1)\mathbf x'F\mathbf x=0\tag{1} x′Fx=0(1)
　　而对本质矩阵，给定一对对应点x↔x′\mathbf x\leftrightarrow\mathbf x'x↔x′，归一化图像点对为x^↔x^′\mathbf{\hat x}\leftrightarrow\mathbf{\hat x'}x^↔x^′，定义为
x^′TEx^=0(2)\mathbf{\hat x'}^TE\mathbf{\hat x}=0\tag{2} x^′TEx^=0(2)
　　把点对应关系x^=K−1x\mathbf{\hat x}=K^{-1}\mathbf{x}x^=K−1x，和x^′=K′−1x′\mathbf{\hat x'}=K'^{-1}\mathbf{x'}x^′=K′−1x′代入(2)可以得到x′TK′−TEK−1x=0\mathbf{ x'}^TK'^{-T}EK^{-1}\mathbf{x}=0x′TK′−TEK−1x=0，则有如下关系
E=K′TFK(3)E=K'^TFK\tag{3} E=K′TFK(3)
　　考虑相机矩阵P=K[I∣0]P=K[I|\mathbf 0]P=K[I∣0]和P′=[R∣t]P'=[R|\mathbf t]P′=[R∣t]，根据FFF矩阵的性质有
F=K′−T[t]×RK−1=K′−TR[RTt]×K−1(4)F=K'^{-T}[\mathbf t]_{\times}RK^{-1}=K'^{-T}R[R^{T}\mathbf t]_{\times}K^{-1}\tag{4} F=K′−T[t]×RK−1=K′−TR[RTt]×K−1(4)
　　从而有
E=[t]×R=R[RTt]×(5)E=[\mathbf t]_{\times}R=R[R^{T}\mathbf t]_{\times}\tag{5} E=[t]×R=R[RTt]×(5)

本质矩阵的性质

其中t\mathbf tt和RRR分别有333个自由度，除去一个齐次因子，EEE只有555个自由度，因此需要满足比FFF矩阵更多的约束。

反对称矩阵的性质

如果一个矩阵MMM是d×dd\times dd×d的反对称矩阵（skew-symmetric/antisymmetric matrices），那么其满足MT=−MM^T=-MMT=−M，有
det⁡M=det⁡(−MT)=det⁡(−M)=(−1)ddet⁡M(6)\det M=\det(-M^T)=\det(-M)=(-1)^d\det M\tag{6} detM=det(−MT)=det(−M)=(−1)ddetM(6)
　　可以观察到，当ddd为奇数时，det⁡M=0\det M=0detM=0。所以，反对称矩阵MMM的秩一定为偶数。

如果MMM的维度为偶数的非奇异2n×2n2n\times 2n2n×2n反对称矩阵，那么，存在正交矩阵UUU，有
UTMU=N≡diag{(0m1−m10),(0m2−m20),...,(0mn−mn0)}(7)U^TMU=N\equiv \text{diag} \begin{Bmatrix} \begin{pmatrix}0&m_1\\-m_1&0\end{pmatrix}, \begin{pmatrix}0&m_2\\-m_2&0\end{pmatrix}, ..., \begin{pmatrix}0&m_n\\-m_n&0\end{pmatrix} \end{Bmatrix}\tag{7} UTMU=N≡diag{(0−m1m10),(0−m2m20),...,(0−mnmn0)}(7)
　　这里的mjm_jmj都是正实数。矩阵NNN称为非奇异反对称矩阵的实标准型（real normal form）。如果矩阵MMM是奇异的且秩为2n2n2n的d×dd\times dd×d矩阵（ddd可以是奇数可以是偶数），则存在d×dd\times dd×d矩阵UUU使得

UTMU=N≡diag{(0m1−m10),(0m2−m20),...,(0mn−mn0),Od−2n}(8)U^TMU=N\equiv \text{diag} \begin{Bmatrix} \begin{pmatrix}0&m_1\\-m_1&0\end{pmatrix}, \begin{pmatrix}0&m_2\\-m_2&0\end{pmatrix}, ..., \begin{pmatrix}0&m_n\\-m_n&0\end{pmatrix}, O_{d-2n} \end{Bmatrix}\tag{8} UTMU=N≡diag{(0−m1m10),(0−m2m20),...,(0−mnmn0),Od−2n}(8)
　　这里的Od−2nO_{d-2n}Od−2n是一个(d−2n)×(d−2n)(d-2n)\times(d-2n)(d−2n)×(d−2n)的零矩阵，当d=2nd=2nd=2n则(8)(8)(8)式退化为(7)(7)(7)式。

综上所述，实反对称矩阵MMM可以分解为M=UNUTM=UNU^TM=UNUT的形式，UUU为正交矩阵，其中NNN是形如diag(m1D,m2D,...,mnD,0,...,0)T\text{diag}(m_1D,m_2D,...,m_nD,0,...,0)^Tdiag(m1D,m2D,...,mnD,0,...,0)T的分块矩阵，其中D=[01−10]D=\begin{bmatrix}0&1\\-1&0\end{bmatrix}D=[0−110]。

本质矩阵的性质

本质矩阵E=[t]×R=SRE=[\mathbf t]_{\times}R=SRE=[t]×R=SR，我们考虑3×33\times33×3反对称矩阵SSS，其可表示为S=kUZUTS=kUZU^TS=kUZUT，根据上述的反对称矩阵的性质，3×33\times33×3的反对称矩阵det⁡S=0\det S=0detS=0，对应上述d=3d=3d=3，n=1n=1n=1的情况，则矩阵ZZZ可以表示为
Z=[010−100000]Z=\begin{bmatrix} 0&1&0\\ -1&0&0\\ 0&0&0 \end{bmatrix} Z=⎣⎡0−10100000⎦⎤
　　在相差一个尺度因子的情况下，E=UZUTRE=UZU^TRE=UZUTR。为把矩阵EEE写成奇异值分解E=UΣVTE=U\Sigma V^TE=UΣVT的形式，则需要把ZZZ构造为一个对角矩阵和正交矩阵相乘的形式。根据初等行变换
[Z∣I]=[010100−100010000001]⇒[1000−10010100000001]=[diag(1,1,0)∣W][Z|I]= \left[ \begin{array}{ccc|ccc} 0&1&0&1&0&0\\ -1&0&0&0&1&0\\ 0&0&0&0&0&1 \end{array}\right] \Rightarrow \left[ \begin{array}{ccc|ccc} 1&0&0&0&-1&0\\ 0&1&0&1&0&0\\ 0&0&0&0&0&1 \end{array}\right]=[\text{diag}(1,1,0)|W] [Z∣I]=⎣⎡0−10100000100010001⎦⎤⇒⎣⎡100010000010−100001⎦⎤=[diag(1,1,0)∣W]
　　观察到这里的WWW是一个正交矩阵且W−1=WTW^{-1}=W^TW−1=WT，有
ZW=diag(1,1,0)ZWT=−diag(1,1,0)(9)\begin{aligned} ZW&=\text{diag}(1,1,0)\\ ZW^T&=-\text{diag}(1,1,0) \end{aligned}\tag{9} ZWZWT=diag(1,1,0)=−diag(1,1,0)(9)
　　因此本质矩阵的奇异值分解可以表示为
E=UZUTR=Udiag(1,1,0)(WTUTR)≡Udiag(1,1,0)V1TE=UZUTR=Udiag(1,1,0)(−WUTR)≡Udiag(1,1,0)V2T(10)\begin{aligned} E&=UZU^TR=U\text{diag}(1,1,0)(W^TU^TR)\equiv U\text{diag}(1,1,0) V_1^T\\ E&=UZU^TR=U\text{diag}(1,1,0)(-WU^TR)\equiv U\text{diag}(1,1,0) V_2^T \end{aligned}\tag{10} EE=UZUTR=Udiag(1,1,0)(WTUTR)≡Udiag(1,1,0)V1T=UZUTR=Udiag(1,1,0)(−WUTR)≡Udiag(1,1,0)V2T(10)

所以本质矩阵分解有两种情况，但都有如下形式
E=Udiag(1,1,0)VT(11)E=U\text{diag}(1,1,0) V^T\tag{11} E=Udiag(1,1,0)VT(11)
　　并且我们有如下结论（性质）

一个矩阵是本质矩阵的充要条件是其奇异值中有两个相等且第三个是000

本质矩阵的分解

我们希望通过本质矩阵的SVD分解得到RRR和t\mathbf tt。考虑本质矩阵两个SVD分解的情况，如果我们通过SVD分解得到
E=UΣVT=Udiag(1,1,0)VTE=U\Sigma V^T=U\text{diag}(1,1,0) V^T E=UΣVT=Udiag(1,1,0)VT
　　设E=SRE=SRE=SR，SSS的形式和上述相同S=UZUTS=UZU^TS=UZUT，则分解得到的旋转矩阵可以记为R=UXVTR=UXV^TR=UXVT，这里的XXX是某个旋转矩阵。则有
Udiag(1,1,0)VT=E=SR=(UZUT)(UXVT)=U(ZX)VTU\text{diag}(1,1,0) V^T=E=SR=(UZU^T)(UXV^T)=U(ZX)V^T Udiag(1,1,0)VT=E=SR=(UZUT)(UXVT)=U(ZX)VT
　　因此有ZX=diag(1,1,0)ZX=\text{diag}(1,1,0)ZX=diag(1,1,0)，从而有X=WX=WX=W或者X=WTX=W^TX=WT，因此旋转矩阵RRR有如下两种情况
R1=UWTVTR2=UWVT(12)\begin{aligned} R_1=UW^TV^T&&R_2=UWV^T\\ \end{aligned}\tag{12} R1=UWTVTR2=UWVT(12)
　　这里回顾(10)(10)(10)式，由于RRR为旋转矩阵，则有det⁡R=1\det R=1detR=1，因此(10)(10)(10)式中det⁡V1=det⁡(RTUW)=det⁡(RT)det⁡(U)det⁡(W)=det⁡(U)\det V_1=\det(R^TUW)=\det(R^T)\det(U)\det(W)=\det(U)detV1=det(RTUW)=det(RT)det(U)det(W)=det(U)，则有det⁡(UV)=1\det(UV)=1det(UV)=1，而对于det⁡V2=det⁡(−RTUWT)=−det(U)\det V_2=\det(-R^TUW^T)=-det(U)detV2=det(−RTUWT)=−det(U)，则有det⁡(UV)=−1\det(UV)=-1det(UV)=−1，所以对应于det⁡(UV)=−1\det(UV)=-1det(UV)=−1的情况，(12)(12)(12)式中求得的旋转矩阵的行列式就为−1-1−1，所以在结果中需要取反。
　　
　　接下来我们考虑t\mathbf tt，根据E=[t]×R=SRE=[\mathbf t]_{\times}R=SRE=[t]×R=SR，即我们从S=[t]×S=[\mathbf t]_\timesS=[t]×中得到t\mathbf tt，考虑
St=[t]×t=0S\mathbf t=[\mathbf t]_\times \mathbf t=0 St=[t]×t=0
　　则t\mathbf tt属于SSS的零空间，通过对前两行的线性变换，可以把式子化为奇异值分解的形式，而考虑到Z的最后一行为零，也就是对应了最小奇异值（0），因而解就是t=U(0,0,1)T=u3\mathbf t=U(0,0,1)^T=\mathbf u_3t=U(0,0,1)T=u3，即UUU最后一列。考虑到给t\mathbf tt乘以一个非零尺度因子得λt\lambda\mathbf tλt，有[λt]×R=λ[t]×R=λE[\lambda\mathbf t]_{\times}R=\lambda[\mathbf t]_{\times}R=\lambda E[λt]×R=λ[t]×R=λE，而对于EEE而言（相差一个尺度因子）这种情况也是等效的，而对于t\mathbf tt而言，当λ=±1\lambda=\pm1λ=±1时，其物理上的意义（方向）却是不同的。所以，不考虑尺度因子，即取∥t∥=1\|\mathbf t\|=1∥t∥=1，t\mathbf tt的方向依然无法确定，所以有两种可能的解。

综上，本质矩阵的分解一共有444种可能的解，即已知本质矩阵E=Udiag(1,1,0)VTE=U\text{diag}(1,1,0) V^TE=Udiag(1,1,0)VT和第一个相机矩阵P=[I∣0]P=[I|\mathbf 0]P=[I∣0]，则第二个相机矩阵P′P'P′有如下444四种可能的解
P′=[UWVT∣u3]or[UWTVT∣u3](λ=1)[UWVT∣−u3]or[UWTVT∣−u3](λ=−1)(13)\begin{aligned} P'=[UWV^T|\mathbf u_3] && \text{or}&&[UW^TV^T|\mathbf u_3] && (\lambda=1)\\ [UWV^T|-\mathbf u_3] && \text{or}&&[UW^TV^T|-\mathbf u_3] && (\lambda=-1) \end{aligned}\tag{13} P′=[UWVT∣u3][UWVT∣−u3]oror[UWTVT∣u3][UWTVT∣−u3](λ=1)(λ=−1)(13)
　　下图的四种情况就是上述四种解对应的两个相机之间的关系。其实这四种情况中只有一种是符合实际的解，只需要根据上述的解根据三角法去计算3D点的坐标，只有当两个相机观测到3D点都在前方，也就是深度都为正，才是最终的解。

下面的是ORB-SLAM2中本质矩阵分解的代码

void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{cv::Mat u,w,vt;cv::SVD::compute(E,w,u,vt);u.col(2).copyTo(t);t=t/cv::norm(t);cv::Mat W(3,3,CV_32F,cv::Scalar(0));W.at<float>(0,1)=-1;W.at<float>(1,0)=1;W.at<float>(2,2)=1;R1 = u*W*vt;if(cv::determinant(R1)<0)R1=-R1;R2 = u*W.t()*vt;if(cv::determinant(R2)<0)R2=-R2;
}

以及OpenCV中的代码

void cv::decomposeEssentialMat( InputArray _E, OutputArray _R1, OutputArray _R2, OutputArray _t )
{Mat E = _E.getMat().reshape(1, 3);CV_Assert(E.cols == 3 && E.rows == 3);Mat D, U, Vt;SVD::compute(E, D, U, Vt);if (determinant(U) < 0) U *= -1.;if (determinant(Vt) < 0) Vt *= -1.;Mat W = (Mat_<double>(3, 3) << 0, 1, 0, -1, 0, 0, 0, 0, 1);W.convertTo(W, E.type());Mat R1, R2, t;R1 = U * W * Vt;R2 = U * W.t() * Vt;t = U.col(2) * 1.0;R1.copyTo(_R1);R2.copyTo(_R2);t.copyTo(_t);
}

之后对四个解做判断的代码在这里，篇幅过长则不贴出

ORB-SLAM2,主要在CheckRT这个函数
OpenCV

参考

Multipe View Geometry in Computer Vision II, 9.6
Properties of antisymmetric matrices
Camera Computation and the Essential Matrix