相机姿态估计

总目标：计算相机运动

2D-2D：对极几何#

对极约束#

$$x{_2}{^T} t^∧Rx_1=0$$

$$p_2^TK^{-T}t^∧RK^{-1}p_1=0$$

它的几何意义是$O_1，P，O_2$三者共面。对极约束中包含了平移和旋转。
定义基础矩阵(Fundamental Matrix)$F$和本质矩阵(Essential Matrix)$E$，

$$E=t^∧R，F=K^{-T}EK^{-1}$$

则对极约束：

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

本质矩阵E的求解#

E的性质：

• 对极约束是等式为零的约束，E乘以任意非零常数后，对极约束依然满足
• $E=t^∧R$，其奇异值是$[\sigma ,\sigma ,0]^T$的形式。
• R和t共有六个自由度，则E也有6自由度。但由于尺度等价性（可以同时乘以常数），E实际上有5个自由度。

八点法#

• 为什么选择八个点，而不是5个点？
  E的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性。E是一个3✖3的矩阵，9个点，考虑尺度等价性后，需要8个点。
• 多于8对点的情况：
  计算最小二乘解，将八点法中的左侧矩阵记为A：
  Ae=0，八点法中A的大小为8✖9，若给定的匹配点多于8，构成超定方程，则不一定能求得e。因此可以最小化一个二次型来求解：

$$min_e||Ae||^2_2=min_ee^TA^TAe$$

• 八点法：
  • 选取8对匹配点，代入对极约束。则可求得E。
  • 再利用SVD分解：
    $$E=U\Sigma V^T$$

  • 求得t,R的可能值：
  $$t_1^∧=UR_Z(\frac{\pi}{2})\Sigma U^T，R_1=UR_Z^T(\frac{pi}{2})V^T$$
  $$t_2^∧=UR_Z(-\frac{\pi}{2})\Sigma U^T，R_2=UR_Z^T(-\frac{\pi}{2})V^T$$
  • $R_Z(\frac{\pi}{2})$表示沿z轴旋转90度得到旋转矩阵。同时E和-E等价，所以也可以对任意一个t取负号。因此，共存在4个解。但只有一个解使得P在两个相机中都具有正的深度。
  • 我们根据线性方程求出的E可能不满足E的内在性质——奇异值可能不为$[\sigma ,\sigma ,0]^T$的形式。因此，我们会刻意将$\Sigma$矩阵调整为上面的样子。
    $$E=U diag(\frac{\sigma_{1}+\sigma_{2}}{2},\frac{\sigma_{1}+\sigma_{2}}{2},0)V^T$$

单应矩阵H#

• 通过假设场景中的特征点都落在同一平面上：
  $$-\frac{n^TP}{d}=1$$
  得到每对匹配点的线性关系。然后选取8对点进行求解H。同样，再将H分解得到R和t。
• 单应性的重要意义：
  当特征点共面或者发生相机纯旋转时，基础矩阵的自由度下降，这就出现了退化(degenerate)。此时，基础矩阵多余出来的自由度将会主要由噪声决定。
  为此，我们会同时估计基础矩阵F和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计。

三角测量#

目的是求出深度。

$$s_2x_2=s_1Rx_1+t$$

同乘$x_2^∧$:

$$s_2x_2^∧x_2=s_1x_2^∧Rx_1+x_2^∧t$$

$$s_2x_2^∧x_2=0$$

变为求解 $$s_1x_2^∧ Rx_1+x_2^∧t=0$$

3D-2D：Pnp#

DLT#

每对匹配点满足

$$t_1^Tp-t_3^Tpu_1=0\\ t_1^Tp-t_3^Tpu_1=0$$

找出6对匹配点，列12个方程求解t（定义[R|t]为一个3✖4的矩阵，$t_1$代表$（t_1,t_2,t_3,t_4)^T$，以此类推），但旋转矩阵需要满足约束，可以由QR分解来求得。

P3P#

通过余弦定理，选择3对匹配点进行运算。

最小化重投影误差(Bundle Adjustment)#

Bundle Adjustment：把相机和三维点放在一起进行最小化的问题。

其中$u_i$表示观测数据，当e为像素坐标误差，x为相机位姿。

3D-3D:ICP(Iterative Cloest Point)#

ICP可以分为以下三个步骤求解：

• 计算两组点的质心位置$p,p^{'}$，然后计算每个点的去质心坐标：

$$q_i=p_i-p，q^{'}_i=p^{'}_i-p^{'}$$

• 根据以下优化问题计算旋转矩阵：

$$R^* = argmin_R\frac{1}{2} \sum_{i=1}^{n} \lvert \rvert q_i - R q^{'}_{i} \lvert \rvert ^2$$

• 根据第2步的R计算t：

$$t^*=p-Rp^{'}$$

SVD求解#

定义$W=\sum_{i=1}^{n} q_iq^{'T}_i$.
W是一个3✖3的矩阵，对W进行SVD分解：

$$W=U\Sigma V^T$$

当W满秩时，R为

$$R=UV^T$$

非线性优化方法#

以李代数表达位姿时，目标函数可以写成：