相机外参初始估计

算法概述

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

1. 数据准备

XXX: 一个 3×N3 \times N3×N 的矩阵，表示 NNN 个世界坐标系下的 3D 点。
pimgp_{img}pimg: 一个 2×N2 \times N2×N 的矩阵，表示对应的 NNN 个图像像素点。

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

2. 共面性检验

步骤 2.1: 中心化 : 计算 3D 点集的质心 Xˉ\bar{X}Xˉ，并对点集进行中心化。
Xˉ=1N∑i=1NXi,X˙=X−Xˉ \bar{X} = \frac{1}{N} \sum_{i=1}^{N} X_i \quad , \quad \dot{X} = X - \bar{X} Xˉ=N1i=1∑NXi,X˙=X−Xˉ

步骤 2.2: SVD 分解 : 计算中心化点云 X˙\dot{X}X˙ 的协方差矩阵 C=X˙X˙TC = \dot{X}\dot{X}^TC=X˙X˙T，并对其进行奇异值分解 (SVD)。
C=UΣUT,Σ=diag(σ1,σ2,σ3) 且 σ1≥σ2≥σ3 C = U \Sigma U^T \quad , \quad \Sigma = \text{diag}(\sigma_1, \sigma_2, \sigma_3) \text{ 且 } \sigma_1 \ge \sigma_2 \ge \sigma_3 C=UΣUT,Σ=diag(σ1,σ2,σ3) 且 σ1≥σ2≥σ3

UUU 的列向量是数据分布的主方向。
σi\sigma_iσi 是在对应主方向上的方差。

步骤 2.3: 判断 : 如果 σ3/σ2<ϵ\sigma_3 / \sigma_2 < \epsilonσ3/σ2<ϵ (一个很小的阈值)，则点集可被视为共面。UUU 的第三列 u3u_3u3 即为该平面的法向量。

3. 坐标系变换

目标 (Objective)

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

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

原理与步骤 (Principle and Steps)

一个刚体变换由旋转和平移两部分组成。我们需要找到一个旋转矩阵 RTR_TRT 和一个平移向量 tTt_TtT，使得变换后的新坐标 XpX_pXp 满足 Xp=RTX+tTX_p = R_T X + t_TXp=RTX+tT，且 XpX_pXp 的第三行为零。

1. 定义新坐标系 PPP 的姿态

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

新坐标系的原点 : 我们选择原始 3D 点云的质心 Xˉ\bar{X}Xˉ 作为新坐标系 PPP 的原点。这是一个非常自然和方便的选择，因为它代表了点集的几何中心。
新坐标系的基向量 : 我们利用 SVD 分解得到的矩阵 U=[u1u2u3]U = \begin{bmatrix} u_1 & u_2 & u_3 \end{bmatrix}U=[u1u2u3] 来定义新坐标系的坐标轴。
- u1,u2u_1, u_2u1,u2: 这两个向量是点集分布方差最大的两个方向，它们构成了点集所在平面的一个正交基。我们将它们作为新坐标系 PPP 的 X 轴和 Y 轴。
- u3u_3u3: 这个向量是点集所在平面的法向量 。我们将它作为新坐标系 PPP 的 Z 轴。

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

2. 计算从世界坐标系 WWW 到新坐标系 PPP 的变换

现在我们需要计算将一个在 WWW 系下的点 XXX 转换为 PPP 系下的点 XpX_pXp 所需的 RTR_TRT 和 tTt_TtT。

旋转矩阵 RTR_TRT

目的 : 将世界坐标系 WWW 的基向量旋转到新坐标系 PPP 的基向量 (u1,u2,u3)(u_1, u_2, u_3)(u1,u2,u3) 上。
计算 : 从 SVD 得到的矩阵 UUU 的列向量 u1,u2,u3u_1, u_2, u_3u1,u2,u3 构成了从 WWW 系的标准基到 PPP 系基的旋转。因此，将一个在 WWW 系中表示的向量转换到 PPP 系中去表示，所需要的旋转矩阵恰好是 UUU 的转置。
RT=UT R_T = U^T RT=UT
验证 : 这个旋转的目的，是把平面法向量 u3u_3u3 旋转成新坐标系的 Z 轴方向 (001)T\begin{pmatrix} 0 & 0 & 1 \end{pmatrix}^T(001)T。我们可以验证一下：

由于 UUU 是正交矩阵，有 UTU=IU^T U = IUTU=I。u3u_3u3 是 UUU 的第三列，所以 UTu3U^T u_3UTu3 的结果必然是单位矩阵 III 的第三列，即：
RTu3=UTu3=(001) R_T u_3 = U^T u_3 = \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} RTu3=UTu3= 001

这证明了 RT=UTR_T=U^TRT=UT 确实完成了我们期望的旋转。
平移向量 tTt_TtT

目的 : 将新坐标系的原点（即点云质心 Xˉ\bar{X}Xˉ）移动到坐标 (0,0,0)(0,0,0)(0,0,0)。
计算 : 一个点 XXX 首先经过旋转，其坐标变为 RTXR_T XRTX。然后我们再进行平移。我们希望质心 Xˉ\bar{X}Xˉ 最终的坐标为零，即：
RTXˉ+tT=0 R_T \bar{X} + t_T = 0 RTXˉ+tT=0

由此直接解得：
tT=−RTXˉ t_T = -R_T \bar{X} tT=−RTXˉ

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

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

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

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

原理:

问题转化 :

经过上一步变换，我们得到了一组 Z=0 的 3D 点 XpX_pXp。我们可以忽略其 Z 分量，将其视为一组 2D 点，并用齐次坐标表示为 X~p\tilde{X}_pX~p (一个 3×N3 \times N3×N 矩阵，第三行为1)。

现在，问题变成了寻找一个 3×33 \times 33×3 的单应性矩阵 HHH，它描述了从平面点 X~p\tilde{X}pX~p 到归一化图像点 ppp 之间的投影关系：
si(pi1)=H(X~p,i1) s_i \begin{pmatrix} p_i \\ 1 \end{pmatrix} = H \begin{pmatrix} \tilde{X}{p,i} \\ 1 \end{pmatrix} si(pi1)=H(X~p,i1)

这里的相机姿态 (R′,t′)(R', t')(R′,t′) 是相机相对于这个新构建的平面坐标系 的姿态。HHH 与该姿态的关系为：
H=λ[r1′r2′t′] H = \lambda \begin{bmatrix} r'_1 & r'_2 & t' \end{bmatrix} H=λ[r1′r2′t′]

其中 R′=[r1′r2′r3′]R' = \begin{bmatrix} r'_1 & r'_2 & r'_3 \end{bmatrix}R′=[r1′r2′r3′]，t′t't′ 是平移向量，λ\lambdaλ 是一个任意尺度因子。
分解 HHH :

设 H=[h1h2h3]H = \begin{bmatrix} h_1 & h_2 & h_3 \end{bmatrix}H=[h1h2h3]。
- 恢复尺度 λ\lambdaλ : 由于 r1′r'_1r1′ 是旋转矩阵的列，它必须是单位向量。因此，∥λr1′∥=λ∥r1′∥=λ\| \lambda r'_1 \| = \lambda \| r'_1 \| = \lambda∥λr1′∥=λ∥r1′∥=λ。我们可以从 h1h_1h1 的范数来估计尺度：
  λ≈∥h1∥ \lambda \approx \|h_1\| λ≈∥h1∥
- 恢复旋转矩阵 R′R'R′ :
  r1′=h1λ r'_1 = \frac{h_1}{\lambda} r1′=λh1
  
  由于噪声影响，h2/λh_2/\lambdah2/λ 可能不与 r1′r'_1r1′ 完全正交。我们需要使用格拉姆-施密特 (Gram-Schmidt) 正交化来强制正交性：
  r2′=h2−(h2Tr1′)r1′∥h2−(h2Tr1′)r1′∥ r'_2 = \frac{h_2 - (h_2^T r'_1) r'_1}{\| h_2 - (h_2^T r'_1) r'_1 \|} r2′=∥h2−(h2Tr1′)r1′∥h2−(h2Tr1′)r1′
  
  旋转矩阵的第三列可以通过前两列的叉积得到：
  r3′=r1′×r2′ r'_3 = r'_1 \times r'_2 r3′=r1′×r2′
  
  至此，我们得到了相机相对于新坐标系的旋转矩阵 R′=[r1′r2′r3′]R' = \begin{bmatrix} r'_1 & r'_2 & r'_3 \end{bmatrix}R′=[r1′r2′r3′]。
- 恢复平移向量 t′t't′ :
  t′=h3λ t' = \frac{h_3}{\lambda} t′=λh3

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

我们已经求得相机相对于平面坐标系的姿态 (R′,t′)(R', t')(R′,t′)，以及平面坐标系相对于世界坐标系的姿态 (RT,tT)(R_T, t_T)(RT,tT)。现在需要计算相机相对于世界坐标系的最终姿态 (R,t)(R, t)(R,t)。

一个世界点 XXX 在相机坐标系下的坐标 XcX_cXc 可以通过两次变换得到：
Xc=R′Xp+t′=R′(RTX+tT)+t′=(R′RT)X+(R′tT+t′) \begin{align*} X_c & = R' X_p + t' \\ & = R' (R_T X + t_T) + t' \\ & = (R' R_T) X + (R' t_T + t') \end{align*} Xc=R′Xp+t′=R′(RTX+tT)+t′=(R′RT)X+(R′tT+t′)

根据相机模型的定义 Xc=RX+tX_c = R X + tXc=RX+t，我们可以得到：
R=R′RT R = R' R_T R=R′RT

t=R′tT+t′ t = R' t_T + t' t=R′tT+t′

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