计算机视觉中的位姿估计

1. 问题描述

位姿估计在计算机视觉领域扮演着十分重要的角色。在使用视觉传感器估计机器人位姿进行控制、机器人导航、增强现实以及其它方面都有着极大的应用。位姿估计这一过程的基础是找到现实世界和图像投影之间的对应点。然后根据这些点对的类型，如2D-2D, 2D-3D, 3D-3D，采取相应的位姿估计方法。我们通常称把根据已知点对估计位姿的过程称为求解PnP。

2. 2D-2D：对极几何

假设我们从两幅图像中，得到了一对配对好的特征点。如下图所示，$p_1$和$p_2$是两幅图像中匹配的特征点，$O_1$、$O_2$是两个相机中心。

如果我们有若干对这样的特征点，我们就可以通过这些二维图像点的对应关系，恢复出两帧之间摄像机的运动。以上图为例，我们希望求取两帧图像$I_1$和$I_2$之间相机的运动。设第一帧到第二帧的运动为$R$，$t$，$P$点在第一帧相机坐标系下的坐标为$P=[X,Y,Z{]}^T$，$P$点在两帧图像的像素坐标系中的坐标为$p_1$、$p_2$，则有下式成立
$$\begin{gathered} s_1{\text{p}}_1=\text{KP} \\ {s}_2{\text{p}}_2=\text{K(RP+t)} \end{gathered}$$
若采用齐次坐标，则上式可以写成
$$\begin{gathered} {\text{p}}_1=\text{KP} \\ {\text{p}}_2=\text{K(RP+t)} \end{gathered}$$

取
$$\begin{gathered} {\text{x}}_1={\text{K}}^{-1}{\text{p}}_1 \\ {\text{x}}_2={\text{K}}^{-1}{\text{p}}_2 \end{gathered}$$
此处$x_1$、$x_2$是两点在归一化平面上的坐标，代入上式可得
$${\text{x}}_2={\text{Rx}}_1+\text{t}$$
两边同时左乘$\text{t}\hat{}$，得到
$$\text{t}\hat{}{\text{x}}_2=\text{t}\hat{}{\text{Rx}}_1$$
两边同时左乘${\text{x}}_2^T$，得到
$$x_2^{T}t\hat{}{x}_2=x_2^{T}t\hat{}R{x}_1$$
因为$t\hat{}{x}_2$与$x_2$垂直，所以上式左侧为$0$，因此
$$x_2^{T}t\hat{}R{x}_1=0$$
将$p_1$、$p_2$代入，得
$$p_2^T{K}^{-T}t\hat{}R{K}^{-1}{p}_1=0$$
上述两式被称为对极约束，它的几何意义是$O_1$、$O_2$、$P$、三点共面。对极约束中同时包含了旋转和平移，我们把中间部分基座两个矩阵：基础矩阵$F$和本质矩阵$E$
$$\begin{gathered} E=t\hat{}R \\ F=K^{-T}E{K}^{-1} \end{gathered}$$
则可以进一步简化对极约束
$$x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$$
于是，相机位姿估计问题变为以下两步：

1.根据配对点的像素位置，求出$E$或者$F$；

2.根据$E$或者$F$，求出$R$，$T$。

3. 三角测量

我们使用对极几何约束估计了相机运动后，需要用相机的运动估计特征点的空间位置。仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量的方法来估计地图点的深度。

考虑图像$I_1$和$I_2$，以左图为参考，右图的变换矩阵为$T$。相机光心为$O_1$和$O_2$。$p_1$和$p_2$为两幅图像中匹配的特征点。理论上直线$O_1{p}_1$与$O_2{p}_2$在场景中会相交于点$P$，该点即是两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交，我们可以通过最二小乘法去求解。

按照对极几何中的定义，设$x_1$，$x_2$为两个特征点的归一化坐标，那么
$$s_1{x}_1=s_{2}R{x}_2+t$$
上式左乘$x_1\hat{}$，得
$$s_2{x}_1\hat{}R{x}_2+x_1\hat{}t=0$$
根据此方程可以求出$s_2$。有了$s_2$，$s_1$也很容易求出。于是，我们就得到了两个帧下的点的深度，确定了它们的空间坐标。当然，由于噪声的存在，我们估得的$R$，$t$不一定精确，所以更常见的做法是求最小二乘解而不是零解。

4. 3D-2D:PnP

PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。

4.1 直接线性变换

考虑某个空间点$P$，它在世界坐标系下的齐次坐标为$\text{P}=(X,Y,Z,1{)}^T$，在归一化平面上的坐标为$\text{x}=(u,v,1{)}^T$。此时相机的位姿$\text{R}，$，$\text{t}$是未知的。我们定义$\text{T}=[\text{R}|\text{t}]$为一个$3\times 4$的矩阵，则
$$s\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$

用最后一行把s消去，得到
$$\begin{gathered} u=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{5}X+t_{6}Y+t_{7}Z+t_8} \\ v=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{5}X+t_{6}Y+t_{7}Z+t_8} \end{gathered}$$
为了简化表示，定义$\text{T}$的行向量为
$$\begin{gathered} {\text{t}}_{\text{1}}={\left(t_1,t_2,t_3,t_4\right)}^T \\ {\text{t}}_{\text{2}}={\left(t_1,t_2,t_3,t_4\right)}^T \\ {\text{t}}_{\text{3}}={\left(t_1,t_2,t_3,t_4\right)}^T \end{gathered}$$
则
$$\begin{gathered} {\text{t}}_1^T\text{P}-{\text{t}}_3^T\text{Pu}=0 \\ {\text{t}}_2^T\text{P}-{\text{t}}_3^T\text{Pv}=0 \end{gathered}$$
可以看到每个特征点提供了两个关于$\text{t}$的线性约束。假设一共有$N$个特征点，可以列出如下线性方程组
$$\left(\begin{array}{ccc}{P}_1^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ ⋮& ⋮& ⋮\\ P_N^T& 0& -u_N{P}_N^T\\ 0& P_N^T& -u_N{P}_N^T\end{array}\right)\left(\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right)=0$$
由于 t 一共有 12 维，因此最少通过六对匹配点，即可实现矩阵 T 的线性求解，这种方法称为直接线性变换（Direct Linear Transform，DLT）。当匹配点大于六对时，可以使用 SVD 等方法对超定方程求最小二乘解。

在 DLT 求解中，我们直接将$T$矩阵看成了12个未知数，忽略了它们之间的联系。因为旋转矩阵为正交矩阵，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵。平移向量比较好办，它属于向量空间。对于旋转矩阵$R$，我们必须针对 DLT 估计的$T$的左边3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。

4.2 P3P

$P3P$是另一种解$PnP$的方法。它仅使用三对匹配点，对数据要求较少。

$P3P$需要利用给定的三个点的几何关系。它的输入数据为三对$3D-2D$匹配点。记 3D点为$A$，$B$，$C$，2D 点为$a$，$b$，$c$，其中小写字母代表的点为大写字母在相机成像平面上的投影，如上图所示。此外，$P3P$还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为$D-d$，相机光心为$O$。

根据余弦定理，有
$$\begin{gathered} O{A}^2+O{B}^2-2OA\cdot OB\cdot \cos ⟨a,b⟩=A{B}^2 \\ O{B}^2+O{C}^2-2OB\cdot OC\cdot \cos ⟨b,c⟩=B{C}^2 \\ O{C}^2+O{A}^2-2OC\cdot OA\cdot \cos ⟨c,a⟩=A{C}^2 \end{gathered}$$
上面三式全体除以$O{C}^2$，并记$x=\frac{OA}{OC}$，$y=\frac{OB}{OC}$，可得
$$\begin{gathered} x^2+y^2-2xy\cos ⟨a,b⟩=\frac{A{B}^2}{O{C}^2} \\ {y}^2+1-2y\cos ⟨b,c⟩=\frac{B{C}^2}{O{C}^2} \\ 1+x^2-2x\cos ⟨c,a⟩=\frac{A{C}^2}{O{C}^2} \end{gathered}$$
记$v=\frac{A{B}^2}{O{C}^2},uv=\frac{B{C}^2}{O{C}^2},wv=\frac{A{C}^2}{O{C}^2}$，则
$$\begin{gathered} x^2+y^2-2xy\cos ⟨a,b⟩-v=0 \\ {y}^2+1-2y\cos ⟨b,c⟩-uv=0 \\ 1+x^2-2x\cos ⟨c,a⟩-wv=0 \end{gathered}$$
我们可以把第一个式子中的$v$放到等式一边，并代入2，3两式，可得
$$\begin{gathered} \left(1-u\right)y^2-u{x}^2-\cos ⟨b,c⟩y+2uxy\cos ⟨a,b⟩+1=0 \\ \left(1-w\right)x^2-w{y}^2-\cos ⟨a,c⟩x+2wxy\cos ⟨a,b⟩+1=0 \end{gathered}$$
由于我们知道 2D 点的图像位置，三个余弦角是已知的。同时， $u=\frac{B{C}^2}{A{B}^2}$， $w=\frac{A{C}^2}{A{B}^2}$ 可以通过$A$，$B$，$C$在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的$x$，$y$是未知的，随着相机移动会发生变化。因此，该方程组是关于$x$，$y$的一个二元二次方程（多项式方程）。