根据匹配点估计相机运动

根据匹配点对估计相机运动

• 2D-2D：对极几何
• 3D-2D：PnP
• 3D-3D：ICP

2D-2D：对极几何

1 对极约束

极线：在对极几何约束模型中，若不知道p与p2，那么p1对应所在I2平面的位置就在极点e2出发的一条射线上，具体就是e2(p2)所在的直线上。（其中，p1,p2都是p在对应平面上的投影点）

实践中已知量：p1,p2 //通过特征匹配确定

未知量：p, e1, e2

待求量：相机的运动 T12，p

约束模型中两个像素点 $p_1,p_2$ 的关系，

$$s_1p_1 = KP_w\;,\;s_2p_2 = K(RP_w+t)$$

其中，s1，s2是世界坐标中的z轴坐标，是一个尺度。前一个等式为P在l1上的直接投影，而后一个等式是在相机内参不变的情况下在前一个等式两边增加一个变换矩阵T，根据P在l1的投影得出相机运动后P在l2的投影。

对极约束关系式：

$$x_2^Tt^\wedge Rx_1=0 \tag{1}$$ $$p_2^TK^{-T}t^\wedge RK^{-1}p_1=0 \tag{2}$$

另 $E=t^\wedge R$（本质矩阵），$F=K^{-T}EK^{-1}$（基础矩阵）

$$\Rightarrow x_2^TEx_1=p_2^TFp_1=0$$

然后通过匹配的特征点对求出E或F，进而求出R，t

2 本质矩阵

八点法求解本质矩阵

通过八对特征点，建立八个方程，将本质矩阵以八个自由度计算，最后再对计算结果优化，使矩阵满足本质矩阵只有五个自由度的性质。

SVD分解求解相机运动 R，t

$$\begin{align} & E=U\Sigma V^T\\ \Rightarrow\;& t_1^\wedge,t_2^\wedge=UR_Z(\pm\frac{\pi}{2})\Sigma U^T\\ & R_1,R_2=UR_Z(\pm\frac{\pi}{2})V^T \end{align}$$

其中，V、U为正交矩阵，$\Sigma=diag(\sigma, \sigma,0)$是一个对角矩阵，最后一维为0表示相差一个尺度，若求出的不满足这个形式，则调整前两维相等，最后一维为零。

3 单应矩阵

三角测量

$$s_2x_2=s_1Rx_1+t\; \Rightarrow\;s_2x_2^\wedge x_2=\underline{0=s_1x_2^\wedge Rx_1+x_2^\wedge t}$$

得到一个超定方程，用最小二乘求解

或者

$$\begin{align}s_2x_2-s_1Rx_1+t \;& \Rightarrow\;\left\{\begin{matrix}s_2x_2^Tx_2-s_1x_2^TRx_1=x_2^Tt\\ s_2(Rx_1)^Tx_2-s_1(Rx_1)^TRx_1=(Rx_1)^Tt\end{matrix}\right.\\&\\& \Rightarrow \begin{bmatrix}x_2^Tx_2 & -x_2^Tx_1\\ (Rx_1)^Tx_2 & -(Rx_1)^TRx_1\end{bmatrix}\begin{bmatrix}s_1\\ s_2\end{bmatrix}=\begin{bmatrix}x_2^Tt\\ (Rx_1)^Tt\end{bmatrix}\end{align} \tag{Cramer's Rule}$$

---

参考资料：

• 视觉SLAM十四讲：从理论到实践（第2版）. 高翔.