本质矩阵可以在网上找到很多相关推导方式,可以很容易的推出其表达式为$\boldsymbol{E}=[\boldsymbol{t}]_\times \boldsymbol{R}=\boldsymbol{SR}$,那么如何根据这个本质矩阵推导出相机位姿?本文参考《计算机视觉中的多视图几何》,并且进行了仔细的推导,特此记录以备后用。

结论

若$\boldsymbol{E}$的SVD分解为$\boldsymbol{E}=\boldsymbol{U}diag(1,1,0)\boldsymbol{V^T}$,记

$$ \boldsymbol{W}=\left[ \begin{array}{cccc} 0&-1&0\\ 1&0&0\\ 0&0&1 \end{array} \right], \boldsymbol{Z}=\left[ \begin{array}{cccc} 0&1&0\\ -1&0&0\\ 0&0&0 \end{array} \right] $$

那么$\boldsymbol{E}=\boldsymbol{SR}$有如下两种可能的分解(忽略符号!)。

$$ \boldsymbol{S}=\boldsymbol{U}\boldsymbol{Z}\boldsymbol{U}^T, \boldsymbol{R}=\boldsymbol{U}\boldsymbol{W}\boldsymbol{V}^T or \boldsymbol{U}\boldsymbol{W}^T\boldsymbol{V}^T $$

证明

证$\boldsymbol{S}=\boldsymbol{U}\boldsymbol{Z}\boldsymbol{U}^T$

这部分在书上一笔带过,$\boldsymbol{S}$的形式是根据它的左零矢量也是$\boldsymbol{E}$的左零矢量这样的实时来确定的。这句话其实有下面几个意思:

  • $\boldsymbol{E}$和$\boldsymbol{S}$的左零空间相等,即存在向量$\boldsymbol{x}$,$\boldsymbol{x^TE}=\boldsymbol{0} \Leftrightarrow \boldsymbol{x^TS}=\boldsymbol{0}$。

    • 这个其实是比较容易理解的。$\boldsymbol{R}$是一个正交阵,$\boldsymbol{E}$和$\boldsymbol{S}$就差了一个$\boldsymbol{R}$,正交阵只改变向量的方向,不改变向量的大小。这样的话,$\boldsymbol{x^TS}=\boldsymbol{0}$,那么$\boldsymbol{x^TE}=\boldsymbol{x^TSR}=\boldsymbol{0}$,反过来更容易推出。
  • $\boldsymbol{S}$的表达式可以根据$\boldsymbol{E}$和$\boldsymbol{S}$的左零空间相等这个前提推得,那么接下来就要推它了。

根据《计算机视觉中的多视图几何》结论A3.1,一般反对称矩阵(那个$[\boldsymbol{t}]_\times]$,或者说$\boldsymbol{S}$),可以写为

$$ \boldsymbol{S} = k\boldsymbol{U}\boldsymbol{Z}\boldsymbol{U}^T $$

记这个$\boldsymbol{U}$为$\boldsymbol{U_1}$即$\boldsymbol{S} = k\boldsymbol{U_1}\boldsymbol{Z}\boldsymbol{U_1}^T$,另一方面由题干$\boldsymbol{E}=\boldsymbol{U}diag(1,1,0)\boldsymbol{V^T}$,记这个$\boldsymbol{U}$为$\boldsymbol{U_2}$,即$\boldsymbol{E}=\boldsymbol{U_2}diag(1,1,0)\boldsymbol{V^T}$。
证$\boldsymbol{U_1}==\boldsymbol{U_2}$:
由$\boldsymbol{E}$和$\boldsymbol{S}$的左零空间相等,把$\boldsymbol{U_1}$和$\boldsymbol{U_1}$分别带进$\boldsymbol{S}$和$\boldsymbol{E}$。
那么存在$\boldsymbol{x}$,使得

$$ \boldsymbol{x^T}k\boldsymbol{U_1}\boldsymbol{Z}\boldsymbol{U_1}^T=\boldsymbol{0} \Leftrightarrow \boldsymbol{x^T}\boldsymbol{U_2}diag(1,1,0)\boldsymbol{V}^T=\boldsymbol{0} $$

等式左边右乘一系列正交阵可以做如下变换:

$$ \boldsymbol{x^T}\boldsymbol{U_1}\boldsymbol{Z}\boldsymbol{U_1}^T=\boldsymbol{0} \Leftrightarrow \boldsymbol{x^T}\boldsymbol{U_1}diag(1,1,0)=\boldsymbol{0} $$

等式右边右乘一系列正交阵可以做如下变换:

$$ \boldsymbol{x^T}\boldsymbol{U_2}diag(1,1,0)\boldsymbol{V}^T=\boldsymbol{0} \Leftrightarrow \boldsymbol{x^T}\boldsymbol{U_2}diag(1,1,0)=\boldsymbol{0} $$

由于上面留个式子等价,所以到这里可以很容易看出来$\boldsymbol{U_1}==\boldsymbol{U_2}$,证的话就是把$\boldsymbol{U_1}$,$\boldsymbol{U_2}$和$\boldsymbol{x}$都拆成列,然后去实际地一乘,去分析它们的关系。由于等式中第三行被乘0忽略,所以只能确定$\boldsymbol{U_1}$,$\boldsymbol{U_2}$的前两行相等。但是$\boldsymbol{U_1}$,$\boldsymbol{U_2}$都是正交阵,所以第三行在前两行确定相等的情况下,必然也是相等的。
由此得证$\boldsymbol{S}=\boldsymbol{U}\boldsymbol{Z}\boldsymbol{U}^T$就是SVD分解$\boldsymbol{E}$得到的$\boldsymbol{U}$。

证$\boldsymbol{R}=\boldsymbol{U}\boldsymbol{W}\boldsymbol{V}^T or \boldsymbol{U}\boldsymbol{W}^T\boldsymbol{V}^T$

其实第二部分是比较好证的,也很简单,在此就直接贴出原文证明吧。
R的恢复

实际应用

注意到上面的证明中$\boldsymbol{S}$的符号是未知的,$\boldsymbol{R}$也有两种可能,这样总共就有四种可能。实际上只有一种情况能够将特征点都投影到两个相机的像平面前面,这样就只需要把四种情况带进去测试一下就可以了。
四种可能的情况
总结起来,要根据$\boldsymbol{E}=\boldsymbol{SR}$恢复出$\boldsymbol{S}$和$\boldsymbol{R}$,要进行如下步骤:

  • 首先进行SVD分解得到$\boldsymbol{E}=\boldsymbol{U}diag(1,1,0)\boldsymbol{V^T}$
  • 构成四种可能的情况,其中$\boldsymbol{u}_3$是$\boldsymbol{U}$的第三列(最后一列):

    • $\boldsymbol{T}_1 = [\boldsymbol{U}\boldsymbol{W}\boldsymbol{V}^T | \boldsymbol{u}_3]$
    • $\boldsymbol{T}_2 = [\boldsymbol{U}\boldsymbol{W}\boldsymbol{V}^T | -\boldsymbol{u}_3]$
    • $\boldsymbol{T}_3 = [\boldsymbol{U}\boldsymbol{W}^T\boldsymbol{V}^T | \boldsymbol{u}_3]$
    • $\boldsymbol{T}_4 = [\boldsymbol{U}\boldsymbol{W}^T\boldsymbol{V}^T | -\boldsymbol{u}_3]$
  • 将$\boldsymbol{T}_1,\boldsymbol{T}_2,\boldsymbol{T}_3,\boldsymbol{T}_4$和两相机都看到的一个世界坐标系特征点进行坐标系变换,找到能使该特征点在两相机坐标系下都在相机平面的正面$\boldsymbol{T}_i$即为最终解。

标签: SLAM

添加新评论