问题描述
假设在 \(d\) 维空间 \(\mathbb{R}^d\) 中，存在两个对应点集合 \(P = \left\{ {{{\mathbf{p}}_1},{{\mathbf{p}}_2}, \cdots ,{{\mathbf{p}}_n}} \right\}\) , \(Q = \left\{ {{{\mathbf{q}}_1},{{\mathbf{q}}_2}, \cdots ,{{\mathbf{q}}_n}} \right\}\)，其中 \(\mathbf{p}_i\) 与 \(\mathbf{q}_i\) 对应。
我们希望找到一个刚体变换使得这两个集合下的点在最小二乘条件下完成对齐。即找到一个旋转矩阵 \({\mathbf{R}}\) 和平移矩阵 \({\mathbf{t}}\) , 满足对齐后的所有点距离平方加权和最小：
\[\left( {{\mathbf{R}},{\mathbf{t}}} \right) = \mathop {\arg \min }\limits_{{\mathbf{R}} \in SO\left( d \right),{\mathbf{t}} \in {\mathbb{R}^d}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {({\mathbf{R}}{{\mathbf{p}}_i} + {\mathbf{t}}) - {{\mathbf{q}}_i}} \right\|}^2}}
\]
求解步骤
经过一系列的复杂推导后，求解 \(\mathbf{R}\) 、\(\mathbf{t}\) 的步骤可以概括为以下几步：
1、计算两个点集的重心：
\[{\mathbf{\bar p}} = \frac{{\sum\nolimits_{i = 1}^n {{w_i}{{\mathbf{p}}_i}} }}{{\sum\nolimits_{i = 1}^n {{w_i}} }},{\mathbf{\bar q}} = \frac{{\sum\nolimits_{i = 1}^n {{w_i}{{\mathbf{q}}_i}} }}{{\sum\nolimits_{i = 1}^n {{w_i}} }}
\]
2、计算去除重心后的新点集合：
\[{{\mathbf{x}}_i}: = {{\mathbf{p}}_i} - {\mathbf{\bar p}},\quad {{\mathbf{y}}_i}: = {{\mathbf{q}}_i} - {\mathbf{\bar q}},\quad i = 1,2, \cdots n.
\]
3、计算 \(d \times d\) 协方差矩阵（covariance matrix）：
\[{\mathbf{S = XW}}{{\mathbf{Y}}^T}
\]
其中，\(\mathbf{X}\) 和 \(\mathbf{Y}\) 分别是以 \(\mathbf{x}_i\) 和\(\mathbf{y}_i\) 为列向量构成的 \(d \times n\) 矩阵，对角矩阵\(\mathbf{W} = diag(w_1,w_2,\cdots,w_n)\) 。
4、SVD 分解矩阵 \(\mathbf{S}\) ：
\[{\mathbf{S = U\Sigma }}{{\mathbf{V}}^T}
\]
5、求解旋转矩阵 \(\mathbf{R}\) ：
\[{\mathbf{R}} = {\mathbf{V}}\left( {\begin{array}{*{20}{l}}
1&{}&{}&{}&{} \\
{}&1&{}&{}&{} \\
{}&{}& \ddots &{}&{} \\
{}&{}&{}&1&{} \\
{}&{}&{}&{}&{\det \left( {{\mathbf{V}}{{\mathbf{U}}^T}} \right)}
\end{array}} \right){{\mathbf{U}}^T}
\]
6、求解平移矩阵 \(\mathbf{t}\) :
\[{\mathbf{t}} = {\mathbf{\bar q}} - {\mathbf{R\bar p}}
\]
推导过程
待补充，