求解两个坐标系的相对旋转和平移矩阵

在三维世界中的两个坐标系 O1 和 O2，如何确定这两个坐标系的相对旋转和平移矩阵？至少需要多少个三维点分别在这两个坐标系下的坐标？

1、需要多少个对应点？

至少需要 3 个非共线的点分别在 O1 和 O2 下的坐标。

情况一：1 个点

• 比喻：你在 O1 板子的原点上画一个点 P1，在 O2 板子上也画一个对应的点 P2。现在，你用一根针穿过 P1 和 P2，把它们固定在一起。
• 结果：两个板子的原点对齐了（平移被确定了）。但是，O2 板子可以围绕这根针进行任意角度的旋转和倾斜。你无法确定它的姿态。
• 结论：1 个点只能固定平移，无法提供任何旋转信息。存在 无数种 可能的旋转。

情况二：2 个点

• 比喻：现在，你在 O1 板子上再增加一个点 Q1，在 O2 板子上也增加对应的点 Q2。你用第二根针穿过 Q1 和 Q2。现在两个板子被两根针固定住了。
• 结果：这两根针就像一个门轴。你可以让 O2 板子 围绕着连接 P 和 Q 的这条直线轴进行旋转。虽然它不能随意倾斜了，但它仍然可以像螺旋桨一样转动。
• 结论：2 个点确定了一条直线，消除了两个旋转自由度，但还剩下 一个自由度——即围绕这条直线的旋转。

情况三：3 个非共线的点

• 比喻：你在 O1 板子上增加第三个点 R1，它不在 P1 和 Q1 构成的直线上。在 O2 板子上也增加对应的点 R2。你用第三根针穿过 R1 和 R2。
• 结果：这三根针形成了一个三角形的支架，将两个板子 完全锁死 了。你再也无法移动或旋转 O2 板子。它们的位置和姿态关系被唯一确定了。
• 结论：3 个非共线的点构成一个平面，这个平面完全固定了坐标系，消除了所有旋转上的不确定性。

为什么必须“非共线”？

如果第三个点 R1 在线段 PQ 上，那么第三根针扎在线段 PQ 上，它并没有提供任何新的约束。这就像在已经有两根合页的门轴上再加一根合页一样，门依然可以绕着门轴转动。所以，三个点在一条直线上和两个点的情况是一样的。

2、需要求解几个参数？

总共需要求解 6 个自由度（DOF）的参数。 [1]

一个刚体变换包括旋转和平移两部分：

1. 平移 (Translation)：由一个三维向量 t 表示，有 3 个分量 (tx, ty, tz)。这部分有 3 个自由度。
2. 旋转 (Rotation)：由一个 3x3 的旋转矩阵 R 表示。虽然这个矩阵有 9 个元素，但它们不是独立的。作为一个正交矩阵（orthonormal matrix），它必须满足以下约束：正交性：RTR=IRTR=I (其中 II 是单位矩阵)方向性：det(R)=+1det(R)=+1 (表示这是一个右手坐标系的旋转，而不是反射)这些约束将旋转矩阵的自由度从 9 个减少到 3 个。这 3 个自由度可以直观地理解为围绕 x, y, z 三个轴的旋转角度（如欧拉角或 roll-pitch-yaw 角）。

因此，总参数数量为：3 (平移) + 3 (旋转) = 6 个参数。

3. 具体求解算法

最经典和常用的方法是基于奇异值分解（SVD）的闭式解法（Closed-form solution）。该方法旨在找到最优的旋转矩阵 R 和平移向量 t，使得两组对应点之间的均方根误差（RMSE）最小。其他方法包括基于四元数的 Horn 方法等。

下面我们将详细推导基于 SVD 的算法。

4. 详细数学推导证明过程 (基于 SVD 的最小二乘法)

假设我们在坐标系 O1 下有一组点 ${p_1, p_2, ..., p_n}$，在坐标系 O2 下有对应的另一组点 ${p'_1, p'_2, ..., p'_n}$。我们的目标是找到一个旋转矩阵 R 和一个平移向量 t，使得对于每个点对，以下关系尽可能成立：

$$
p'i \approx R p_i + t
$$
目标： 最小化误差的平方和，即最小化目标函数 $E(R, t)$:
$$
E(R, t) = \sum^{n} | p'_i - (R p_i + t) |^2
$$

第一步：解耦平移向量 t 和旋转矩阵 R

一个关键的技巧是，最优的平移向量 t 可以用旋转矩阵 R 和两组点的质心来表示。首先，我们计算两组点的质心（或称为均值点）：

$$
\mu_p = \frac{1}{n} \sum_{i = 1}^{n} p_i
$$

$$
\mu_{p'} = \frac{1}{n} \sum_{i = 1}^{n} p'_i
$$

为了找到使 $E(R, t)$ 最小的 t，我们对 t 求偏导并令其为零：

$$
\frac{\partial E}{\partial t} = \sum_{i = 1}^{n} 2(p'_i - R p_i - t)(-1) = 0
$$

$$
\sum_{i = 1}^{n} (p'_i - R p_i - t) = 0
$$

$$
\sum_{i = 1}^{n} p'i - R \sum^{n} p_i - \sum_{i = 1}^{n} t = 0
$$

$$
n \mu_{p'} - R (n \mu_p) - n t = 0
$$

由此，我们得到最优平移向量 t 的表达式：

$$
t = \mu_{p'} - R \mu_p
$$
这个公式的直观意义是：两个坐标系的相对平移，等于它们质心之间的差值，再减去质心因旋转而产生的位移。

现在，我们将这个 t 的表达式代回到原始的目标函数中，以消除 t，得到一个只关于 R 的新目标函数。为此，我们先定义“去中心化”的坐标：

$$
q_i = p_i - \mu_p
$$

$$
q'_i = p'i - \mu
$$

代入 $t = \mu_{p'} - R \mu_p$ 到 $p'_i - R p_i - t$ 中：
$$
p'i - R p_i - (\mu - R \mu_p) = (p'i - \mu) - R(p_i - \mu_p) = q'_i - R q_i
$$
于是，目标函数简化为：

$$
E(R) = \sum_{i = 1}^{n} | q'_i - R q_i |^2
$$

第二步：使用 SVD 求解旋转矩阵 R

我们现在需要找到一个旋转矩阵 R 来最小化 $E(R)$。我们展开上式：

$$
\begin{aligned}
E(R) &= \sum_{i = 1}^{n} (q'_i - R q_i)^T (q'i - R q_i)\
&=\sum^{n} ( (q'_i)^T q'_i - (q'_i)^T R q_i - (R q_i)^T q'_i + (R q_i)^T (R q_i) )
\end{aligned}
$$
因为 R 是正交矩阵，所以 $(R q_i)^T (R q_i) = q_i^T R^T R q_i = q_i^T I q_i = q_i^T q_i$。同时，$(R q_i)^T q'_i = (q'_i)^TRq_i$。因此：

$$
E(R) = \sum_{i = 1}^{n} ( |q'_i|^2 + |q_i|^2 - 2(q'_i)^T R q_i )
$$
$\sum |q'_i|^2$ 和 $\sum |q_i|^2$ 是与 R 无关的常数。因此，最小化 $E(R)$ 等价于最大化下式：

$$
\sum_{i = 1}^{n} (q'_i)^T R q_i
$$
利用矩阵迹（trace）的性质，我们可以将求和写成迹的形式。定义两个矩阵 $Q$ 和 $Q'$，它们的列分别是去中心化的向量 $q_i$ 和 $q'_i$。

$$
Q = [q_1, q_2, ..., q_n]
$$

$$
Q' = [q'_1, q'_2, ..., q_n]
$$

那么，上述求和可以表示为：

$$
\text{Tr}(R^T Q' Q^T)
$$
现在，我们定义一个 3x3 的“协方差矩阵” $H$：

$$
H = Q (Q')^T = \sum_{i = 1}^{n} q_i (q'_i)^T
$$
我们的目标变为最大化 $\text{Tr}(R H^T)$。

对矩阵 $H$ 进行奇异值分解（SVD）：$H = U S V^T$

其中 $U$ 和 $V$ 是 3x3 的正交矩阵，$S$ 是一个对角矩阵，对角线上的值为 $H$ 的奇异值。代入目标函数：

$$
\text{Tr}(R H^T) = \text{Tr}(R (U S V^T)T) = \text{Tr}(R V S^T U^T)
$$
由于 $S$ 是对角阵，$S^T=S$。利用迹的循环不变性 $\text{Tr}(ABC) = \text{Tr}(CAB)$：

$$
\text{Tr}(R V S U^T) = \text{Tr}(U^T R V S)
$$
令 $M = U^T R V$。因为 $U, R, V$ 都是正交矩阵，它们的乘积 $M$ 也是一个正交矩阵。我们的目标是最大化 $\text{Tr}(M S)$。由于 $S$ 是对角阵，$\text{Tr}(M S) = \sum_{j=1}^{3} M_{jj} S_{jj}$。为了使这个和最大，并且考虑到 $M$ 的列向量是单位向量（因此 $|M_{jj}| \le 1$），我们需要让 $M$ 尽可能接近单位矩阵 $I$。当 $M=I$ 时，$\text{Tr}(I S)$ 取得最大值。

所以，我们令 $M = I$：

$U^T R V = I$

$U^T R V V^T = I V^T \implies U^T R = V^T$

$U U^T R = U V^T \implies R = U V^T$

处理反射情况（特殊情况）：
SVD 分解可能会得出一个反射矩阵（即 $det(R) = -1$），而不是一个纯旋转矩阵。这种情况在有噪声的数据中可能出现。一个纯旋转矩阵必须满足 $det(R) = +1$。

我们有 $det(R) = det(U V^T) = det(U) det(V)$。如果 $det(R) = -1$，我们需要对其进行修正。修正方法是：

$R = U \begin{pmatrix} 1 & 0 & 0 \ 0 & 1 & 0 \ 0 & 0 & det(U V^T) \end{pmatrix} V^T$

这相当于将 $V$ 的最后一列反向，然后再计算 $R$。这样可以确保最终得到的 $R$ 是一个真正的旋转矩阵，并且仍然是该问题最优的解。

第三步：计算最终的平移向量 t

一旦计算出最优的旋转矩阵 $R$，就可以使用第一步中得到的公式来计算平移向量 $t$：

$$
t = \mu_{p'} - R \mu_p
$$
至此，我们已经求得了唯一的旋转矩阵 $R$ 和平移向量 $t$。

总结：算法步骤

1. 计算质心：分别计算两组对应点 ${p_i}$ 和 ${p'i}$ 的质心 $\mu_p$ 和 $\mu$。
2. 去中心化：计算去中心化的点集 ${q_i = p_i - \mu_p}$ 和 ${q'_i = p'i - \mu{p'}}$。
3. 计算协方差矩阵：计算 $H = \sum_{i=1}^{n} q_i (q'_i)^T$。
4. SVD 分解：对 $H$ 进行奇异值分解，得到 $H = U S V^T$。
5. 计算旋转矩阵：计算旋转矩阵 $R = U V^T$。检查 $det(R)$，如果为-1，则通过翻转 $U$ 的最后一列来修正 $R$（即 $R = U \text{diag}(1, 1, -1) V^T$）。
6. 计算平移向量：使用公式 $t = \mu_{p'} - R \mu_p$ 计算平移向量。