相机外参初始估计

相机外参初始估计

算法概述

通过一组 3D-2D 匹配点对 $(X_i,p_i)$，估计相机外参 $(R,t)$。其核心策略是：首先通过主成分分析 (PCA) 判断 3D 点集 $X$ 是否共面。若共面，则将问题转化为一个更简单的 2D-2D 单应性 (Homography) 映射问题，并从中恢复出最终的 3D 姿态。

---

1. 数据准备

• $X$: 一个 $3\times N$ 的矩阵，表示 $N$ 个世界坐标系下的 3D 点。
• $p_{img}$: 一个 $2\times N$ 的矩阵，表示对应的 $N$ 个图像像素点。

归一化: 像素点 $p_{img}$ 通过相机内参 $K$ 的逆和畸变矫正函数，变换为归一化相机平面上的点。我们用 $p$ (一个 $2\times N$ 的矩阵) 表示这些归一化后的点。

---

2. 共面性检验

步骤 2.1: 中心化: 计算 3D 点集的质心 $\bar{X}$，并对点集进行中心化。
$$\bar{X}=\frac{1}{N}\sum _{i=1}^N{X}_i,\dot{X}=X-\bar{X}$$

步骤 2.2: SVD 分解: 计算中心化点云 $\dot{X}$ 的协方差矩阵 $C=\dot{X}{\dot{X}}^T$，并对其进行奇异值分解 (SVD)。
$$C=U\Sigma U^T,\Sigma =\text{diag}({\sigma }_1,{\sigma }_2,{\sigma }_3)\text{ 且 }{\sigma }_1\ge {\sigma }_2\ge {\sigma }_3$$

• $U$ 的列向量是数据分布的主方向。
• ${\sigma }_i$ 是在对应主方向上的方差。

步骤 2.3: 判断: 如果 ${\sigma }_3/{\sigma }_2<ϵ$ (一个很小的阈值)，则点集可被视为共面。$U$ 的第三列 $u_3$ 即为该平面的法向量。

---

3. 坐标系变换

目标 (Objective)

我们的核心目标是进行一次坐标系变换，将原始的世界坐标系（我们称之为 $W$）变换到一个新的、经过精心选择的坐标系（我们称之为 $P$）。选择这个新坐标系 $P$ 的原则是：让所有三维点 $X$ 在这个新坐标系 $P$ 中的 Z 坐标都为零。

实现这个目标后，一个复杂的三维空间点投影问题，就被简化成了一个二维平面到二维图像的映射问题，从而为使用单应性矩阵 (Homography) 铺平了道路。

原理与步骤 (Principle and Steps)

一个刚体变换由旋转和平移两部分组成。我们需要找到一个旋转矩阵 $R_T$ 和一个平移向量 $t_T$，使得变换后的新坐标 $X_p$ 满足 $X_p=R_{T}X+t_T$，且 $X_p$ 的第三行为零。

1. 定义新坐标系 $P$ 的姿态

一个坐标系由它的原点和基向量（坐标轴方向）唯一确定。

• 新坐标系的原点: 我们选择原始 3D 点云的质心 $\bar{X}$ 作为新坐标系 $P$ 的原点。这是一个非常自然和方便的选择，因为它代表了点集的几何中心。

• 新坐标系的基向量: 我们利用 SVD 分解得到的矩阵 $U=\left[\begin{array}{ccc}{u}_1& u_2& u_3\end{array}\right]$ 来定义新坐标系的坐标轴。

  • $u_1,u_2$: 这两个向量是点集分布方差最大的两个方向，它们构成了点集所在平面的一个正交基。我们将它们作为新坐标系 $P$ 的 X 轴和 Y 轴。
  • $u_3$: 这个向量是点集所在平面的法向量。我们将它作为新坐标系 $P$ 的 Z 轴。

有了原点和坐标轴，新坐标系 $P$ 就被完全定义了。它是一个以点云质心为中心，以点云主方向为坐标轴的坐标系。

2. 计算从世界坐标系 $W$ 到新坐标系 $P$ 的变换

现在我们需要计算将一个在 $W$ 系下的点 $X$ 转换为 $P$ 系下的点 $X_p$ 所需的 $R_T$ 和 $t_T$。

• 旋转矩阵 $R_T$

  目的: 将世界坐标系 $W$ 的基向量旋转到新坐标系 $P$ 的基向量 $(u_1,u_2,u_3)$ 上。
  计算: 从 SVD 得到的矩阵 $U$ 的列向量 $u_1,u_2,u_3$ 构成了从 $W$ 系的标准基到 $P$ 系基的旋转。因此，将一个在 $W$ 系中表示的向量转换到 $P$ 系中去表示，所需要的旋转矩阵恰好是 $U$ 的转置。
  $$R_T=U^T$$
  验证: 这个旋转的目的，是把平面法向量 $u_3$ 旋转成新坐标系的 Z 轴方向 ${\left(\begin{array}{ccc}0& 0& 1\end{array}\right)}^T$。我们可以验证一下：
  由于 $U$ 是正交矩阵，有 $U^{T}U=I$。$u_3$ 是 $U$ 的第三列，所以 $U^T{u}_3$ 的结果必然是单位矩阵 $I$ 的第三列，即：
  $$R_T{u}_3=U^T{u}_3=\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$$
  这证明了 $R_T=U^T$ 确实完成了我们期望的旋转。

• 平移向量 $t_T$

  目的: 将新坐标系的原点（即点云质心 $\bar{X}$）移动到坐标 $(0,0,0)$。
  计算: 一个点 $X$ 首先经过旋转，其坐标变为 $R_{T}X$。然后我们再进行平移。我们希望质心 $\bar{X}$ 最终的坐标为零，即：
  $$R_T\bar{X}+t_T=0$$
  由此直接解得：
  $$t_T=-R_T\bar{X}$$

变换的最终形式与结果 (Final Form and Result)

综合旋转与平移，将世界坐标系 $W$ 下的点 $X$ 变换到新坐标系 $P$ 下的点 $X_p$ 的完整公式为：
$$X_p=R_{T}X+t_T$$
最终结果: 经过这个变换，我们得到的新坐标矩阵 $X_p$ 的第三行所有元素都近似为零。这成功地将一个三维空间中的共面点问题，降维成了一个二维平面上的点分布问题，为下一步应用单应性矩阵求解姿态奠定了基础。

---

4. 单应性矩阵计算与分解

目标: 求解从新坐标系的 XY 平面到相机归一化图像平面的单应性映射 $H$，并从 $H$ 中分解出相机姿态。

原理:

1. 问题转化:
   经过上一步变换，我们得到了一组 Z=0 的 3D 点 $X_p$。我们可以忽略其 Z 分量，将其视为一组 2D 点，并用齐次坐标表示为 ${\tilde{X}}_p$ (一个 $3\times N$ 矩阵，第三行为1)。
   现在，问题变成了寻找一个 $3\times 3$ 的单应性矩阵 $H$，它描述了从平面点 ${\tilde{X}}_p$ 到归一化图像点 $p$ 之间的投影关系：
   $$s_i\left(\begin{array}{c}{p}_i\\ 1\end{array}\right)=H\left(\begin{array}{c}{\tilde{X}}_{p,i}\\ 1\end{array}\right)$$
   这里的相机姿态 $(R^{\prime },t^{\prime })$ 是相机相对于这个新构建的平面坐标系的姿态。$H$ 与该姿态的关系为：
   $$H=\lambda \left[\begin{array}{ccc}{r}_1^{\prime }& r_2^{\prime }& t^{\prime }\end{array}\right]$$
   其中 $R^{\prime }=\left[\begin{array}{ccc}{r}_1^{\prime }& r_2^{\prime }& r_3^{\prime }\end{array}\right]$，$t^{\prime }$ 是平移向量，$\lambda$ 是一个任意尺度因子。

2. 分解 $H$:
   设 $H=\left[\begin{array}{ccc}{h}_1& h_2& h_3\end{array}\right]$。

   • 恢复尺度 $\lambda$: 由于 $r_1^{\prime }$ 是旋转矩阵的列，它必须是单位向量。因此，$\Vert \lambda r_1^{\prime }\Vert =\lambda \Vert r_1^{\prime }\Vert =\lambda$。我们可以从 $h_1$ 的范数来估计尺度：
     $$\lambda \approx \Vert h_1\Vert$$

   • 恢复旋转矩阵 $R^{\prime }$:
     $$r_1^{\prime }=\frac{h_1}{\lambda }$$
     由于噪声影响，$h_2/\lambda$ 可能不与 $r_1^{\prime }$ 完全正交。我们需要使用格拉姆-施密特 (Gram-Schmidt) 正交化来强制正交性：
     $$r_2^{\prime }=\frac{h_2-(h_2^T{r}_1^{\prime })r_1^{\prime }}{\Vert h_2-(h_2^T{r}_1^{\prime })r_1^{\prime }\Vert }$$
     旋转矩阵的第三列可以通过前两列的叉积得到：
     $$r_3^{\prime }=r_1^{\prime }\times r_2^{\prime }$$
     至此，我们得到了相机相对于新坐标系的旋转矩阵 $R^{\prime }=\left[\begin{array}{ccc}{r}_1^{\prime }& r_2^{\prime }& r_3^{\prime }\end{array}\right]$。

   • 恢复平移向量 $t^{\prime }$:
     $$t^{\prime }=\frac{h_3}{\lambda }$$

---

5. 姿态转换回原始世界坐标系

我们已经求得相机相对于平面坐标系的姿态 $(R^{\prime },t^{\prime })$，以及平面坐标系相对于世界坐标系的姿态 $(R_T,t_T)$。现在需要计算相机相对于世界坐标系的最终姿态 $(R,t)$。

一个世界点 $X$ 在相机坐标系下的坐标 $X_c$ 可以通过两次变换得到：
$$\begin{array}{rl}{X}_c& =R^{\prime }{X}_p+t^{\prime }\\ & =R^{\prime }(R_{T}X+t_T)+t^{\prime }\\ & =(R^{\prime }{R}_T)X+(R^{\prime }{t}_T+t^{\prime })\end{array}$$
根据相机模型的定义 $X_c=RX+t$，我们可以得到：
$$R=R^{\prime }{R}_T$$

$$t=R^{\prime }{t}_T+t^{\prime }$$

这就是最终输出的相机旋转矩阵和平移向量。