从本质矩阵恢复相机矩阵

本质矩阵

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

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

　　对于一对对应点x↔x′基本矩阵定义为

x′Fx=0(1)

　　而对本质矩阵，给定一对对应点x↔x′，归一化图像点对为x^↔x^′，定义为

x^′TEx^=0(2)

　　把点对应关系x^=K−1x，和x^′=K′−1x′代入(2)可以得到x^′TK′−TEK−1x^=0，则有如下关系

E=K′TFK(3)

　　考虑相机矩阵P=K[I|0]和P′=[R|t]，根据F矩阵的性质有

F=K′−T[t]×RK−1=K′−TR[RTt]×K−1(4)

　　从而有

E=[t]×R=R[RTt]×(5)

本质矩阵的性质

　　其中t和R分别有3个自由度，除去一个齐次因子，E只有5个自由度，因此需要满足比F矩阵更多的约束。

反对称矩阵的性质

　　如果一个矩阵M是d×d的反对称矩阵（skew-symmetric/antisymmetric matrices），那么其满足MT=−M，有

detM=det(−MT)=det(−M)=(−1)ddetM(6)

　　可以观察到，当d为奇数时，detM=0。所以，反对称矩阵M的秩一定为偶数。

　　如果M的维度为偶数的非奇异2n×2n反对称矩阵，那么，存在正交矩阵U，有

UTMU=N≡diag{(0−m1m10),(0−m2m20),...,(0−mnmn0)}(7)

　　这里的mj都是正实数。矩阵N称为非奇异反对称矩阵的实标准型（real normal form）。如果矩阵M是奇异的且秩为2n的d×d矩阵（d可以是奇数可以是偶数），则存在d×d矩阵U使得

UTMU=N≡diag{(0−m1m10),(0−m2m20),...,(0−mnmn0),Od−2n}(8)

　　这里的Od−2n是一个(d−2n)×(d−2n)的零矩阵，当d=2n则(8)式退化为(7)式。

综上所述，实反对称矩阵M可以分解为M=UNUT的形式，U为正交矩阵，其中N是形如diag(m1D,m2D,...,mnD,0,...,0)T的分块矩阵，其中D=[0−110]。

本质矩阵的性质

　　本质矩阵E=[t]×R=SR，我们考虑3×3反对称矩阵S，其可表示为S=kUZUT，根据上述的反对称矩阵的性质，3×3的反对称矩阵detS=0，对应上述d=3，n=1的情况，则矩阵Z可以表示为

Z=⎡⎣⎢0−10100000⎤⎦⎥

　　在相差一个尺度因子的情况下，E=UZUTR。为把矩阵E写成奇异值分解E=UΣVT的形式，则需要把Z构造为一个对角矩阵和正交矩阵相乘的形式。根据初等行变换

[Z|I]=⎡⎣⎢⎢0−10100000100010001⎤⎦⎥⎥⇒⎡⎣⎢⎢100010000010−100001⎤⎦⎥⎥=[diag(1,1,0)|W]

　　观察到这里的W是一个正交矩阵且W−1=WT=−W，有

ZWZWT=diag(1,1,0)=−diag(1,1,0)(9)

　　因此本质矩阵的奇异值分解可以表示为

EE=UZUTR=Udiag(1,1,0)(WTUTR)≡Udiag(1,1,0)VT1=UZUTR=Udiag(1,1,0)(−WUTR)≡Udiag(1,1,0)VT2(10)

　　所以本质矩阵分解有两种情况，但都有如下形式

E=Udiag(1,1,0)VT(11)

　　并且我们有如下结论（性质）

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

本质矩阵的分解

　　我们希望通过本质矩阵的SVD分解得到R和t。考虑本质矩阵两个SVD分解的情况，如果我们通过SVD分解得到

E=UΣVT=Udiag(1,1,0)VT

　　设E=SR，S的形式和上述相同S=UZUT，则分解得到的旋转矩阵可以记为R=UXVT，这里的X是某个旋转矩阵。则有

Udiag(1,1,0)VT=E=SR=(UZUT)(UXVT)=U(ZX)VT

　　因此有ZX=diag(1,1,0)，从而有X=W或者X=WT，因此旋转矩阵R有如下两种情况

R1=UWTVTR2=UWVT(12)

　　这里回顾(10)式，由于R为旋转矩阵，则有detR=1，因此(10)式中detV1=det(RTUW)=det(RT)det(U)det(W)=det(U)，则有det(UV)=1，而对于detV2=det(−RTUWT)=−det(U)，则有det(UV)=−1，所以对应于det(UV)=−1的情况，(12)式中求得的旋转矩阵的行列式就为−1，所以在结果中需要取反。

　　接下来我们考虑t，根据E=[t]×R=SR，即我们从S=[t]x中得到t，考虑

St=[t]×t=0

　　则t属于S的零空间，有St=UZUTt，则UTt=0，所以t=(0,0,1)UT=u3，即U最后一列。考虑到给t乘以一个非零尺度因子得λt，有[λt]×R=λ[t]×R=λE，而对于E而言（相差一个尺度因子）这种情况也是等效的，而对于t而言，当λ=±1时，其物理上的意义（方向）却是不同的。所以，不考虑尺度因子，即取∥t∥=1，t的方向依然无法确定，所以有两种可能的解。

　　综上，本质矩阵的分解一共有4种可能的解，即已知本质矩阵E=Udiag(1,1,0)VT和第一个相机矩阵P=[I|0]，则第二个相机矩阵P′有如下4四种可能的解

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