从本质矩阵恢复相机矩阵

本质矩阵

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

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

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

x′Fx=0(1) (1) x ′ F x = 0

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

x^′TEx^=0(2) (2) x ^ ′ T E x ^ = 0

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

E=K′TFK(3) (3) E = K ′ T F K

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

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

　　从而有

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

本质矩阵的性质

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

反对称矩阵的性质

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

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

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

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

UTMU=N≡diag{(0−m1m10),(0−m2m20),...,(0−mnmn0)}(7) (7) U T M U = N ≡ diag { ( 0 m 1 − m 1 0 ) , ( 0 m 2 − m 2 0 ) , . . . , ( 0 m n − m n 0 ) }

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

UTMU=N≡diag{(0−m1m10),(0−m2m20),...,(0−mnmn0),Od−2n}(8) (8) U T M U = N ≡ diag { ( 0 m 1 − m 1 0 ) , ( 0 m 2 − m 2 0 ) , . . . , ( 0 m n − m n 0 ) , O d − 2 n }

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

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

本质矩阵的性质

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

Z=⎡⎣⎢0−10100000⎤⎦⎥ Z = [ 0 1 0 − 1 0 0 0 0 0 ]

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

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

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

ZWZWT=diag(1,1,0)=−diag(1,1,0)(2)(3)(9) (9) (2) Z W = diag ( 1 , 1 , 0 ) (3) Z W T = − diag ( 1 , 1 , 0 )

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

EE=UZUTR=Udiag(1,1,0)(WTUTR)≡Udiag(1,1,0)VT1=UZUTR=Udiag(1,1,0)(−WUTR)≡Udiag(1,1,0)VT2(4)(5)(10) (10) (4) E = U Z U T R = U diag ( 1 , 1 , 0 ) ( W T U T R ) ≡ U diag ( 1 , 1 , 0 ) V 1 T (5) E = U Z U T R = U diag ( 1 , 1 , 0 ) ( − W U T R ) ≡ U diag ( 1 , 1 , 0 ) V 2 T

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

E=Udiag(1,1,0)VT(11) (11) E = U diag ( 1 , 1 , 0 ) V T

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

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

本质矩阵的分解

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

E=UΣVT=Udiag(1,1,0)VT E = U Σ V T = U diag ( 1 , 1 , 0 ) V T

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

Udiag(1,1,0)VT=E=SR=(UZUT)(UXVT)=U(ZX)VT U diag ( 1 , 1 , 0 ) V T = E = S R = ( U Z U T ) ( U X V T ) = U ( Z X ) V T

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

R1=UWTVTR2=UWVT(150)(12) (12) (150) R 1 = U W T V T R 2 = U W V T

　　这里回顾 (10) ( 10 ) 式，由于 R R 为旋转矩阵，则有 detR=1 det R = 1 ，因此 (10) ( 10 ) 式中 detV1=det(RTUW)=det(RT)det(U)det(W)=det(U) det V 1 = det ( R T U W ) = det ( R T ) det ( U ) det ( W ) = det ( U ) ，则有 det(UV)=1 det ( U V ) = 1 ，而对于 detV2=det(−RTUWT)=−det(U) det V 2 = det ( − R T U W T ) = − d e t ( U ) ，则有 det(UV)=−1 det ( U V ) = − 1 ，所以对应于 det(UV)=−1 det ( U V ) = − 1 的情况， (12) ( 12 ) 式中求得的旋转矩阵的行列式就为 −1 − 1 ，所以在结果中需要取反。
　　
　　接下来我们考虑 t t ，根据 E=[t]×R=SR E = [ t ] × R = S R ，即我们从 S=[t]× S = [ t ] × 中得到 t t ，考虑

St=[t]×t=0 S t = [ t ] × t = 0

　　则 t t 属于 S S 的零空间，通过对前两行的线性变换，可以把式子化为奇异值分解的形式，而考虑到Z的最后一行为零，也就是对应了最小奇异值（0），因而解就是 t=U(0,0,1)T=u3 t = U ( 0 , 0 , 1 ) T = u 3 ，即 U U 最后一列。考虑到给 t t 乘以一个非零尺度因子得 λt λ t ，有 [λt]×R=λ[t]×R=λE [ λ t ] × R = λ [ t ] × R = λ E ，而对于 E E 而言（相差一个尺度因子）这种情况也是等效的，而对于 t t 而言，当 λ=±1 λ = ± 1 时，其物理上的意义（方向）却是不同的。所以，不考虑尺度因子，即取 ∥t∥=1 ‖ t ‖ = 1 ， t t 的方向依然无法确定，所以有两种可能的解。

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

P′=[UWVT|u3][UWVT|−u3]oror[UWTVT|u3][UWTVT|−u3](λ=1)(λ=−1)(151)(152)(13) (13) (151) P ′ = [ U W V T | u 3 ] or [ U W T V T | u 3 ] ( λ = 1 ) (152) [ U W V T | − u 3 ] or [ U W T V T | − u 3 ] ( λ = − 1 )

　　下图的四种情况就是上述四种解对应的两个相机之间的关系。其实这四种情况中只有一种是符合实际的解，只需要根据上述的解根据三角法去计算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(0,1)=-1;
    W.at(1,0)=1;
    W.at(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_(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