Essential Matrix 的求解算法--Nister 五点算法以及原理

转自知乎龙睛尤洋

1. 原理

Nister的论文主要是奇奇怪怪的符号比较多，比较容易理解错。翻了下之前看的时候的笔记，分享出来，看看会不会有帮助。

阅读本文前，你需要有基本的矩阵，数值和多视几何的知识。

首先，你得构造约束。这个大家都知道 essential matrix的定义决定了

${{\bf{q}}^{\bf{T}}}{\bf{Eq}} = {\bf{0}}$

如果你把E的9个成员变量完全展开，就会得到一个线性方程。

每有一个匹配点，你就可以得到一个线性方程，现在你用的是5点法，所以你有5个方程。

但是E是3乘3的矩阵，有9个成员变量，你只有五个线性方程，所以你会得到一个线性解空间，这个解空间有4个自由度。

所以我们会得到线性解空间

${\bf{E}} = x{\bf{X}} + y{\bf{Y}} + z{\bf{Z}} + w{\bf{W}}$

这里要多说几句，这里的 ${\bf{X}},{\bf{Y}},{\bf{Z}},{\bf{W}}$ ，都是3乘3的矩阵，而且他们的值从你构造的五个方程组里，已经知道了。这儿的未知数是 $x,y,z,w$ 这四个系数，你只需要把这四个系数定下来，这个本质矩阵就唯一确定了。

这里需要强调两点：

1，这四个系数无论怎么取，你都能得到一个满足五个方程约束的E，但是E要成为一个essential matrix，还需要满足一些特殊的依赖，这些依赖稍后会说。

2，由于你构造的是一个齐次方程组，它的解包含一个可以任意缩放的尺度， ${\bf{Ax}} = {\bf{0}}$ 则 $s{\bf{Ax}} = {\bf{0}}$ ，而且如果你了解essential matrix的性质，应该知道，它本身的尺度也是可以自由缩放的。因此，不失一般性，我们可以让 $w = 1$ ,这样你只有三个需要求解的未知数 $x,y,z$ 。

接下来你需要做的，就是构造额外的约束，来把这三个系数 $x,y,z$ 确定下来。

靠啥来构造约束呢？

靠essential matrix的一个性质

${\bf{E}}{{\bf{E}}^{\rm{T}}}{\bf{E}} - \frac{1}{2}tr({\bf{E}}{{\bf{E}}^T}){\bf{E}} = {\bf{0}}$ ，这个性质可以由本质矩阵的定义 ${\bf{E = [t}}{{\bf{]}}_{\bf{ \times }}}{\bf{R}}$ 推导出来，怎么推导之后再说，现在先直接拿来用。

我们把 ${\bf{E}} = x{\bf{X}} + y{\bf{Y}} + z{\bf{Z}} + {\bf{W}}$ 带入到这个等式，就可以构造出一个关于 $x,y,z$ 的三阶多项式。再次强调三点：

1，这里的 ${\bf{X}},{\bf{Y}},{\bf{Z}},{\bf{W}}$ ，都是3乘3的矩阵，是齐次方程组的解空间的基，之前已经知道了，未知数只有 $x,y,z$ 。

2，由于E是3 * 3的矩阵，他们经过乘法运算之后要等于0，相当于每一个元素都要等于0，一共可以得到九个方程。

3， ${\bf{E}}{{\bf{E}}^{\rm{T}}}{\bf{E}} - \frac{1}{2}tr({\bf{E}}{{\bf{E}}^T}){\bf{E}} = {\bf{0}}$ ，这个方程里面，因为矩阵乘了三次，所以最高会有三次项，也就是我们会得到9个方程，每个方程都是三元三次的。

所以现在我们有九个三元三次方程构成的齐次线性方程组，这样的方程组是有唯一解的，但是毕竟是三元三次的，不太好解，所以整个论文最绕的部分来了。

先看这个很魔性的图

这个图要怎么理解呢？

先看第一行，这一行是我们三元三次方程，对应的各个多项式子项，这些子项我们记为向量 ${\bf{\tilde x}}$ ；

再看第一列，这一列是我们几个方程对应的代号，（a）代表的是从上到下的第一个方程，（i）代表第9个方程，依次类推；

再看右下角这个大型矩阵，我们记为 ${\bf{A}}$ ，其实就是我们9个齐次方程组的系数矩阵，部分主元消元之后的造型。这个造型有一些需要说明的地方。对角线的区域都是1；所有 . 和大写字母LMNOPQRS对应的区域，都是非零系数项；所有空白的区域，都是系数0；[3] 代表关于z的一个三阶多项式，也就是 $a{z^3} + b{z^2} + cz + d$ 这种形式；注意系数矩阵 ${\bf{A}}$ 的右上部分，也是有一块区域为0的，这个很重要。

三元三次方程不好解，我们的思路是先用换元或者其他的方法，构造出一元多次方程，解出其中一个未知数，比如z，再把这个值代入，三元方程就变成了2元方程，就好解了。

为了进一步往前走，我们再确定两个事情。

首先，我们的9个方程会构成一个齐次方程组 ${\bf{A\tilde x}} = {\bf{0}}$ ，单拧出来任意一行，每个方程也都为0，也就是（a）=0；（b）=0;....;(i)=0

然后，我们看第（i）个方程，实际上他可以写成下面这样的形式

$(i) = xy[1] + x[2] + y[2] + [3] = 0$

再次提醒，这里的[1],[2],[3] 分别代表z的1阶，2阶，3阶多项式。除了z，关于x和y，只有三个系数项xy, x, y。我们实际上可以再构造几个关于关于xy, x, y的齐次方程，这样我们就能想办法消元了。

论文中构造的第一个方程是（j）

$(j) = (e) - z(g) = xy[1] + x[3] + y[3] + [4] = 0$

（j）=0是因为（e）=0 且（g）=0。

这样（j）也是关于系数项xy, x, y的一个齐次方程

类似的方式，构造关于系数项 xy, x, y的其次方程

$(k) = xy[1] + x[3] + y[3] + [4] = 0$

$(l) = xy[2] + x[3] + y[3] + [4] = 0$

$(m) = xy[2] + x[3] + y[3] + [4] = 0$

又要注意了，这里有很多个[1],[2],[3],[4]，每一个分别代表一个z的多项式。
通过(i)到（m）这几个方程，都是关于xy,x,y,1的方程，我们可以把它写成系数矩阵的形式

令 ${\bf{\hat x}} = {(xy,x,y,1)^T}$

我们有5个方程，实际上可以构造2个 4 * 4 的系数矩阵，分别定义为B 和 C

显然我们有齐次线性方程组

${\bf{B\hat x}} = {\bf{0}}$ 和 ${\bf{C\hat x}} = {\bf{0}}$

这两个系数矩阵B和C中的每一个元素，都是z的多项式。而且这两个齐次方程组，都是有非零解的，注意 ${\bf{\hat x}} = {(xy,x,y,1)^T}$ 至少有一个非零项。所以系数矩阵，B和C的行列式，应该都是0，或者说他们肯定是非满秩的。

计算4 * 4矩阵的行列式计算，不是太难，但是由于每一个元素都是关于z的多项式，行列式本身，也是z的多项式。我们记为

$\begin{array}{*{20}{c}} {(n) = \det ({\bf{B}}) = [11]=0}\\ {(o) = \det ({\bf{C}}) = [11]=0} \end{array}$

实际上我们已经搞出了两个只跟z有关的方程，但是这个方程是一个11阶多项式。

为啥是11阶，因为行列式的值是不同行不同列乘加的结果，B和C中最高可以搞出11次项。

当然，两个11阶齐次方程，之间，如果把11阶项消元了，就可以得到一个10阶多项式

$(p) \equiv (n){o_{11}} - (o){n_{11}} = [10] = 0$

怎么解这个10元一次方程呢？论文中的意思是用Sturm sequence去一点一点逼近。关于这个，给个传送门Sturm's theorem - Wikipedia。基本求解的思路是用Sturm的理论，可以确定一个多项式，在一个区间[a,b]之间的符号变化。如果在[a,b]直接有符号变化，则在这个区间一定是有解的，一点一点缩小区域去逼近即可。

其实到这里，这篇论文应该已经看懂了，当然10阶多项式的解多半不止一个，之后需要把每一个z的解，带回到 ${\bf{B\hat x}} = {\bf{0}}$ 和 ${\bf{C\hat x}} = {\bf{0}}$ ，解出x和y吗，然后对应于每一个x,y,z组合，你都会得到一个本质矩阵。

要挑选出正确的那个本质矩阵，你需要把本质矩阵分解成R，T，如果不知道怎么分，可以去看多视几何那本书。有了RT，你就可以拿RT来三角化每一组匹配点，得到他们的三维空间坐标。然后把这个坐标投影到两侧相机的坐标系，其中任意一个坐标系下，如果z是负的，说明这组解是不科学的。因为相机没办法看到身后的物体。这样你就可以筛选出正确的解了。需要注意本质矩阵乘以一个尺度之后，含义不会发生变化。

2. matlab程序

（An Efficient Solution to the Five-Point Relative Pose Problem）

[R_true, ~] = qr(randn(3));
t_true = randn(3,1);
t_true = t_true / norm(t_true);
tx = [0 -t_true(3) t_true(2);
t_true(3) 0 -t_true(1);
-t_true(2) t_true(1) 0];

K = eye(3);
E_true = K' * tx * R_true * K;

disp('ground truth t:');
disp(t_true');

disp('ground truth R:');
disp(R_true);

P = randn(5, 3);
P1 = (eye(3) * P')';
P2 = (R_true * P' + t_true)';

q = [P1(:,1).*P2(:,1) P1(:,2).*P2(:,1) P1(:,3).*P2(:,1) ...
P1(:,1).*P2(:,2) P1(:,2).*P2(:,2) P1(:,3).*P2(:,2) ...
P1(:,1).*P2(:,3) P1(:,2).*P2(:,3) P1(:,3).*P2(:,3)];
[~,~,V] = svd(q);

% in x, y, z, 1 order
E_poly = V(:,6:9);
% 11, 12, 13, 21, 22, 23, 31, 32, 33
detE = o2(o1(E_poly(2,:), E_poly(6,:)) - o1(E_poly(3,:), E_poly(5,:)), E_poly(7,:)) + ...
o2(o1(E_poly(3,:), E_poly(4,:)) - o1(E_poly(1,:), E_poly(6,:)), E_poly(8,:)) + ...
o2(o1(E_poly(1,:), E_poly(5,:)) - o1(E_poly(2,:), E_poly(4,:)), E_poly(9,:));

EET = zeros(3,3,10);
for i = 1:3
    for j = 1:3
        for k = 1:3
            EET(i,j,:) = squeeze(EET(i,j,:)) + o1(E_poly((i-1)*3+k,:), E_poly((j-1)*3+k,:));
        end
    end
end

eta = zeros(3,3,10);
for i = 1:3
    for j = 1:3
        eta(i,j,:) = EET(i,j,:);
        if i == j
            for k = 1:3
                eta(i,j,:) = eta(i,j,:) - 0.5*EET(k,k,:);
            end
        end
    end
end

etaE = zeros(3,3,20);
for i = 1:3
    for j = 1:3
        for k = 1:3
            etaE(i,j,:) = squeeze(etaE(i,j,:)) + o2(squeeze(eta(i,k,:)), E_poly((k-1)*3+j,:));
        end
    end
end

A = [detE'; reshape(etaE, [9, 20])];
A = rref(A);

% if [x y 1] is the solution to A(5:10,11:13 14:16 17:20),
% then [x y 1] is the solution to B;
% which means {sol(A(5:10,11:13 14:16 17:20))} is contained in {sol(B)}

% up to tenth degree
B = zeros(3, 3, 11);
for k = 1:3
B(k,:,:) = [[0 0 0 0 0 0 0 0 A(3+2*k,11:13)]; [0 0 0 0 0 0 0 0 A(3+2*k,14:16)]; [0 0 0 0 0 0 0 A(3+2*k,17:20)]] - ...
[[0 0 0 0 0 0 0 A(4+2*k,11:13) 0]; [0 0 0 0 0 0 0 A(4+2*k,14:16) 0]; [0 0 0 0 0 0 A(4+2*k,17:20) 0]];
end

detB = oz(oz(squeeze(B(1,2,:)), squeeze(B(2,3,:)))-oz(squeeze(B(1,3,:)), squeeze(B(2,2,:))), squeeze(B(3,1,:))) + ...
oz(oz(squeeze(B(1,3,:)), squeeze(B(2,1,:)))-oz(squeeze(B(1,1,:)), squeeze(B(2,3,:))), squeeze(B(3,2,:))) + ...
oz(oz(squeeze(B(1,1,:)), squeeze(B(2,2,:)))-oz(squeeze(B(1,2,:)), squeeze(B(2,1,:))), squeeze(B(3,3,:)));

z = roots(detB);
% z = z(:,1);
z_cand = real(z(abs(imag(z)) < 1e-7));

% one of the results is the true solution
for i = 1:length(z_cand)
z = z_cand(i);

B_recovered = reshape(reshape(B, [9, 11]) * [0 0 0 0 0 0 z^4 z^3 z^2 z^1 1]', [3, 3]);

    % sanity check
    % this should be near zero
    detB_recovered = det(B_recovered);

    [~,~,V] = svd(B_recovered);
    x = V(1,3)/V(3,3);
    y = V(2,3)/V(3,3);

E_recovered = reshape(E_poly * [x;y;z;1], [3, 3])';
[U,S,V] = svd(E_recovered);

    % enforce positive U, V
    if det(U) < 0
        E_recovered = -E_recovered;
    end
    if det(V) < 0
        E_recovered = -E_recovered;
    end
    t_recovered = U(:,3);
    disp('find one possible solution (up to scale):');
    disp('t:');
    disp(t_recovered');
    D = [0 1 0;-1 0 0;0 0 1];
    disp('possible R1:');
    Ra_recovered = U * D * V';
    disp(Ra_recovered);
    disp('possible R2:');
    Rb_recovered = U * D' * V';
    disp(Rb_recovered);

    % sanity check
    % these two should be near zero
    detE_recovered = det(E_recovered);
    constraint = E_recovered*E_recovered'*E_recovered - 0.5 * trace(E_recovered*E_recovered')*E_recovered;
end

% give two x, y, z, 1, return product in x^2, y^2, z^2, xy, xz, yz, x, y,
% z, 1 order
function r = o1(a,b)
    p = a' * b;
    r = [p(1,1) p(2,2) p(3,3) p(1,2)+p(2,1) p(1,3)+p(3,1) p(2,3)+p(3,2) ...
        p(1,4)+p(4,1) p(2,4)+p(4,2) p(3,4)+p(4,3) p(4,4)]';
end

% give x^2, y^2, z^2, xy, xz, yz, x, y, z, 1 and x, y, z, 1
% return x^3, y^3, x^2y, xy^2, x^2z, x^2, y^2z, y^2, xyz, xy, z^2x, zx, x,
% z^2y, zy, y, z^3, z^2, z, 1
function r = o2(a,b)
    p = b' * a';
    r = [p(1,1) p(2,2) p(1,4)+p(2,1) p(1,2)+p(2,4) ...
        p(1,5)+p(3,1) p(1,7)+p(4,1) p(2,6)+p(3,2) ...
        p(2,8)+p(4,2) p(1,6)+p(2,5)+p(3,4) p(1,8)+p(2,7)+p(4,4) ...
        p(1,3)+p(3,5) p(1,9)+p(4,5)+p(3,7) p(1,10)+p(4,7) p(2,3)+p(3,6) ...
        p(2,9)+p(3,8)+p(4,6) p(2,10)+p(4,8) p(3,3) p(3,9)+p(4,3) ...
        p(3,10)+p(4,9) p(4,10)]';
end

% mutliply two z polynomials
function r = oz(a,b)
    da = 11-find(logical(a),1,'first');
    db = 11-find(logical(b),1,'first');
    p = a * b';
    r = zeros(11,1);
    for i = 0:da
        for j = 0:db
            r(11-i-j) = r(11-i-j)+p(11-i,11-j);
        end
    end
end