序号	内容
1	【数理知识】自由度 degree of freedom 及自由度的计算方法
2	【数理知识】刚体 rigid body 及刚体的运动
3	【数理知识】刚体基本运动，平动，转动
4	【数理知识】向量数乘，内积，外积，matlab代码实现
5	【数理知识】协方差，随机变量的的协方差，随机变量分别是单个数字和向量时的协方差
6	【数理知识】旋转矩阵的推导过程，基于向量的旋转来实现，同时解决欧式变换的非线性局限
7	【数理知识】已知 N>=3 个点在前后时刻的坐标，求刚体平移矩阵，旋转矩阵，且这 N>=3 点间距离始终不变代表一个刚体

存在有 $N\ge 3$ 个点，它们两两之间距离始终不变，这就满足了可代表一个刚体的条件。同时，已知这 $N\ge 3$ 个点在前后时刻的坐标，如何求对应刚体的平移矩阵，旋转矩阵？

如下图所示，对应点的颜色相同，$R$ 是旋转，$t$ 是平移。我们希望找到能将数据集 $A$ 中的点对齐到数据集 $B$ 的最佳旋转和平移。这种变换有时被称为欧几里得变换（Euclidean）或刚性变换（Rigid transform），因为它保留了形状和大小。这与仿射变换不同，后者包括缩放和剪切。

这个问题尤其出现在三维点云数据注册等任务中，因为这些数据是从三维激光扫描仪或流行的 Kinect 设备等硬件中获取的。

接下来的描述中，我们为了和图中情况保持一致，我们都假设 $N=3$。

1 解决流程

旋转和平移的方程式可以表示为如下形式：

$$\begin{array}{r}RA+t=B\end{array}$$

最终目的是求取最合适的 $R$ 和 $t$。

至于为什么可以这么表示，请参考文章开头所提及到的其他文章。

1. 找寻质心（Centroid）

这一步也比较简单，质心就是 $N=3$ 个数据点的平均值

$$\begin{array}{rl}{\text{Centroid}}_A& =\frac{1}{3}\sum _{k=1}^3{A}_k\\ {\text{Centroid}}_B& =\frac{1}{3}\sum _{k=1}^3{B}_k\end{array}$$

其中 $A_k$ 和 $B_k$ 分别表示在数据集 $A$ 和 $B$ 中第 $k$ 个数据点的坐标。

---

2. 奇异值分解（SVD）

有几种方法可以找到点之间的最佳旋转。最简单的方法是使用奇异值分解（SVD），因为许多编程语言（Matlab、Octave、使用 LAPACK 的 C 语言、使用 OpenCV 的 C++ 语言…）都可以使用这个函数。SVD 就像线性代数中的一根神奇魔杖，可以解决各种数值问题。这里不会详细介绍它的工作原理，而会介绍如何使用它。你只需要知道，SVD 可以将一个矩阵（称作 $E$）分解/因式分解为另外 3 个矩阵，即

$$\begin{array}{rl}[U,S,V]& =\text{SVD}(E)\\ E& =US{V}^{\text{T}}\end{array}$$

如果 $E$ 是方阵，那么 $U、S$ 和 $V$ 的大小也相同。

3. 通过协方差矩阵得到旋转矩阵

要找到最佳旋转方式，我们首先要重新调整两个数据集的中心，使两个中心点都位于原点，如下图所示。

这样就去除了平移部分，只剩下旋转部分需要处理。下一步是累加一个矩阵（称为 $H$），然后使用 SVD 求出旋转，如下所示：

$$\begin{array}{rl}H& =(A-{\text{Centroid}}_A)(B-{\text{Centroid}}_B{)}^{\text{T}}\\ [U,S,V]& =\text{SVD}(H)\\ R& =V{U}^{\text{T}}\end{array}$$

其中，$H$ 是我们熟悉的协方差矩阵。$A-{\text{Centroid}}_A$ 是用 $A$ 减去 ${\text{Centroid}}_A$ 中的每一列的操作。

需要注意的一点是，要正确计算 $H$。它最终应该是一个 $3\times 3$ 矩阵，而不是一个 $N\times N$ 矩阵（这里 $N$ 是指点的数量，而 $3$ 是指数据的坐标 $[x,y,z]$ 维度是 $3$）。注意转置符号。它是在两个矩阵之间进行乘法运算，这两个矩阵的实际维数分别是 $3\times N$ 和 $N\times 3$。乘法的顺序也很重要，如果换一种方法，就会变成是从 $B$ 到 $A$ 的旋转。

4. 计算平移矩阵

得到旋转矩阵 $R$ 后，平移矩阵 $t$ 也就变得简单了。把质心代入开篇咱们提到的方程，那么有

$$\begin{array}{rl}RA+t& =B\\ R\times {\text{Centroid}}_A+t& ={\text{Centroid}}_B\\ t& ={\text{Centroid}}_B-R\times {\text{Centroid}}_A\end{array}$$

---

2 举例验证 1

假设有 $3$ 个相对位置保持不变的点，已知它们在数据集合 $A$ 和数据集合 $B$ 中的位置，然后计算旋转矩阵 $R$ 和平动矩阵 $t$。

在数据集合 $A$ 中：
点 $1$ 的位置为：$A_1=(1,2,3)$；
点 $2$ 的位置为：$A_2=(4,5,6)$；
点 $3$ 的位置为：$A_3=(7,8,9)$。

在数据集合 $B$ 中：
点 $1$ 的位置为：$B_1=(2,3,4)$；
点 $2$ 的位置为：$B_2=(5,6,7)$；
点 $3$ 的位置为：$B_3=(8,9,10)$。

计算思路为：

• 先计算平移：通过求取这些点在两个时刻的质心位置，然后求差来得到平移矩阵

1. 找寻质心（Centroid）

这一步也比较简单，直接代入样本数据

$$\begin{array}{rl}{\text{Centroid}}_A& =\frac{(1+4+7,2+5+8,3+6+9)}{3}=(\frac{1+4+7}{3},\frac{2+5+8}{3},\frac{3+6+9}{3})=(4,5,6)\\ {\text{Centroid}}_B& =\frac{(2+5+8,3+6+9,4+7+10)}{3}=(\frac{2+5+8}{3},\frac{3+6+9}{3},\frac{4+7+10}{3})=(5,6,7)\end{array}$$

2. 计算协方差矩阵

根据公式

$$\begin{array}{rl}\text{Cov}(X,Y{)}_{ij}& =\frac{{\sum }_k^{n=3}(x_{ki}-{\bar{x}}_i)(y_{kj}-{\bar{y}}_i)}{n-1}\end{array}$$

可以得到协方差矩阵

$$\begin{array}{rl}\text{Cov}(X,Y)& =\left[\begin{array}{ccc}3& 3& 3\\ 3& 3& 3\\ 3& 3& 3\end{array}\right]\end{array}$$

关于协方差矩阵的原理和求解方法，可参考文章：【数理知识】协方差，随机变量的的协方差，随机变量分别是单个数字和向量时的协方差。

3. 奇异值分解

可以直接使用 Matlab 进行奇异值分解，可以得到

$$U=\left[\begin{array}{ccc}-0.5774& 0.8165& -0.0000\\ -0.5774& -0.4082& -0.7071\\ -0.5774& -0.4082& 0.7071\end{array}\right],S=\left[\begin{array}{ccc}9& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right],V=\left[\begin{array}{ccc}-0.5774& 0.8165& 0\\ -0.5774& -0.4082& -0.7071\\ -0.5774& -0.4082& 0.7071\end{array}\right]$$

$$R=V{U}^{\text{T}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

至此得到了旋转矩阵。

4. 计算平移矩阵

$$\begin{array}{rlr}t& ={\text{Centroid}}_B-R\times {\text{Centroid}}_A& =(1,1,1)\end{array}$$

---

Ref

1. FINDING OPTIMAL ROTATION AND TRANSLATION BETWEEN CORRESPONDING 3D POINTS
2. 从3组对应点中求得最佳的旋转和平移变换