问题描述
已知在 \(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}}
\]
其中 \(w_i\) 表示第 \(i\) 对点的权重，已知。
求解步骤
经过一系列的复杂推导后，求解 \(\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}} }},\quad{\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}}
\]
推导过程
消掉平移矩阵
令目标函数的自变量为平移矩阵 \(\mathbf{t}\) ，即：
\[F\left( {\mathbf{t}} \right) = \sum\limits_{i = 1}^n {{w_i}{{\left\| {({\mathbf{R}}{{\mathbf{p}}_i} + {\mathbf{t}}) - {{\mathbf{q}}_i}} \right\|}^2}}
\]
当 \(\frac{{\partial F}}{{\partial {\mathbf{t}}}} = 0\) 时，函数有最小值，于是有
\[0 = \sum\limits_{i = 1}^n {2{w_i}\left( {{\mathbf{R}}{{\mathbf{p}}_i} + {\mathbf{t}} - {{\mathbf{q}}_i}} \right)} = 2{\mathbf{t}}\left( {\sum\limits_{i = 1}^n {2{w_i}} } \right) + 2{\mathbf{R}}\left( {\sum\limits_{i = 1}^n {{w_i}} {{\mathbf{p}}_i}} \right) - 2\sum\limits_{i = 1}^n {{w_i}{{\mathbf{q}}_i}}
\]
令：
\[{\mathbf{\bar p}} = \frac{{\sum\nolimits_{i = 1}^n {{w_i}{{\mathbf{p}}_i}} }}{{\sum\nolimits_{i = 1}^n {{w_i}} }},\quad{\mathbf{\bar q}} = \frac{{\sum\nolimits_{i = 1}^n {{w_i}{{\mathbf{q}}_i}} }}{{\sum\nolimits_{i = 1}^n {{w_i}} }}
\]
则此时平移矩阵可表示为：
\[{\mathbf{t}} = {\mathbf{\bar q}} - {\mathbf{R\bar p}}
\]
带入目标函数后有，消去平移矩阵，变成关于旋转矩阵 \(\mathbf{R}\) 的表达式：
\[\begin{align*}
& \sum\limits_{i = 1}^n {{w_i}{{\left\| {({\mathbf{R}}{{\mathbf{p}}_i} + {\mathbf{t}}) - {{\mathbf{q}}_i}} \right\|}^2}} \\
& = \sum\limits_{i = 1}^n {{w_i}{{\left\| {{\mathbf{R}}{{\mathbf{p}}_i} + {\mathbf{\bar q}} - {\mathbf{R\bar p}} - {{\mathbf{q}}_i}} \right\|}^2}} \\
& = \sum\limits_{i = 1}^n {{w_i}{{\left\| {{\mathbf{R}}({{\mathbf{p}}_i} - {\mathbf{\bar p}}) - ({{\mathbf{q}}_i} - {\mathbf{\bar q}})} \right\|}^2}} \\
\end{align*}
\]
令：
\[{{\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.
\]
则目标函数可重新定义为：
\[\mathbf{R} = \mathop {\arg \min }\limits_{\mathbf{R}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right\|}^2}}
\]
目标函数化简为矩阵的迹
将下式进行化简：
\[\begin{align*}
& {\left\| {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right\|^2} \\
& = {\left( {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right)^T}\left( {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right) \\
& = \left( {{\mathbf{x}}_i^T{{\mathbf{R}}^T} - {\mathbf{y}}_i^T} \right)\left( {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right) \\
& = {\mathbf{x}}_i^T{{\mathbf{R}}^T}{\mathbf{R}}{{\mathbf{x}}_i} - {\mathbf{y}}_i^T{\mathbf{R}}{{\mathbf{x}}_i} - {\mathbf{x}}_i^T{{\mathbf{R}}^T}{{\mathbf{y}}_i} + {\mathbf{y}}_i^T{{\mathbf{y}}_i} \\
\end{align*}
\]
其中，根据旋转矩阵的正交性性质可知，\({{\mathbf{R}}^T}{\mathbf{R}} = {\mathbf{I}}\) 。
\(\mathbf{x}_i\) 和 \(\mathbf{y}_i\) 都是 \(d \times 1\) 的列向量，\(\mathbf{R}\) 为 \(d \times d\) 的方阵，且对于常量 \(a\) 有 \(a^T=a\)。所以有：
\[{\mathbf{x}}_i^T{{\mathbf{R}}^T}{{\mathbf{y}}_i} = {\left( {{\mathbf{x}}_i^T{{\mathbf{R}}^T}{{\mathbf{y}}_i}} \right)^T} = {\mathbf{y}}_i^T{\mathbf{R}}{{\mathbf{x}}_i}
\]
\[{\left\| {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right\|^2} = {\mathbf{x}}_i^T{{\mathbf{x}}_i} - 2{\mathbf{y}}_i^T{\mathbf{R}}{{\mathbf{x}}_i} + {\mathbf{y}}_i^T{{\mathbf{y}}_i}
\]
于是目标函数可以化简为：
\[\begin{align*}
\mathbf{R} &= \mathop{\arg \min }\limits_{\mathbf{R}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right\|}^2}} \\
& = \mathop {\arg \min }\limits_{\mathbf{R}} (\sum\limits_{i = 1}^n {{w_i}{\mathbf{x}}_i^T{{\mathbf{x}}_i}} - \sum\limits_{i = 1}^n {{w_i}2{\mathbf{y}}_i^T{\mathbf{R}}{{\mathbf{x}}_i}} + \sum\limits_{i = 1}^n {{w_i}{\mathbf{y}}_i^T{{\mathbf{y}}_i}} ) \hfill \\
& = \mathop {\arg \min }\limits_{\mathbf{R}} ( - \sum\limits_{i = 1}^n {{w_i}2{\mathbf{y}}_i^T{\mathbf{R}}{{\mathbf{x}}_i}} ) \hfill \\
& = \mathop {\arg \max }\limits_{\mathbf{R}} (\sum\limits_{i = 1}^n {{w_i}{\mathbf{y}}_i^T{\mathbf{R}}{{\mathbf{x}}_i}} ) \hfill \\
\end{align*}
\]
观察可以发现可以用矩阵的迹来表达上式：
\[\begin{align*}
{\mathbf{W}}{{\mathbf{Y}}^T}{\mathbf{RX}} & = \left( {\begin{array}{*{20}{c}}
{{w_1}}&{}&{}&{} \\
{}&{{w_2}}&{}&{} \\
{}&{}& \ddots &{} \\
{}&{}&{}&{{w_n}}
\end{array}} \right)\left( {\begin{array}{*{20}{c}}
{{\mathbf{y}}_1^T} \\
{{\mathbf{y}}_2^T} \\
\vdots \\
{{\mathbf{y}}_n^T}
\end{array}} \right){\mathbf{R}}\left( {\begin{array}{*{20}{c}}
{{{\mathbf{x}}_1}}&{{{\mathbf{x}}_2}}& \cdots &{{{\mathbf{x}}_n}}
\end{array}} \right) \hfill \\
& = \left( {\begin{array}{*{20}{c}}
{{w_1}{\mathbf{y}}_1^T{\mathbf{R}}{{\mathbf{x}}_1}}&{}&{}&{} \\
{}&{{w_2}{\mathbf{y}}_2^T{\mathbf{R}}{{\mathbf{x}}_2}}&{}&{} \\
{}&{}& \ddots &{} \\
{}&{}&{}&{{w_n}{\mathbf{y}}_n^T{\mathbf{R}}{{\mathbf{x}}_n}}
\end{array}} \right) \hfill \\
\end{align*}
\]
故目标函数进一步化简为：
\[\begin{align*}
{\mathbf{R}} &= \mathop {\arg \max }\limits_{\mathbf{R}} (\sum\limits_{i = 1}^n {{w_i}{\mathbf{y}}_i^T{\mathbf{R}}{{\mathbf{x}}_i})} \hfill \\
& = \mathop {\arg \max }\limits_{\mathbf{R}} (tr\left( {{\mathbf{W}}{{\mathbf{Y}}^T}{\mathbf{RX}}} \right)) \hfill \\
\end{align*}
\]
根据矩阵的迹的性质，有：
\[tr({\mathbf{AB}}) = tr({\mathbf{BA}})
\]
于是，目标函数最终化简为：
\[\begin{align*}
{\mathbf{R}} & = \mathop {\arg \max }\limits_{\mathbf{R}} (tr\left( {{\mathbf{W}}{{\mathbf{Y}}^T}{\mathbf{RX}}} \right)) \hfill \\
& = \mathop {\arg \max }\limits_{\mathbf{R}} (tr\left( {({\mathbf{W}}{{\mathbf{Y}}^T})({\mathbf{RX}})} \right)) \hfill \\
& = \mathop {\arg \max }\limits_{\mathbf{R}} (tr\left( {{\mathbf{RXW}}{{\mathbf{Y}}^T}} \right)) \hfill \\
\end{align*}
\]
矩阵的迹的最大值求解
利用SVD分解 \({\mathbf{XW}}{{\mathbf{Y}}^T}\) ，则有：
\[{\mathbf{XW}}{{\mathbf{Y}}^T} ={\mathbf{S}} = {\mathbf{U\Sigma }}{{\mathbf{V}}^T}
\]
\[tr\left( {{\mathbf{RXW}}{{\mathbf{Y}}^T}} \right) = tr\left( {{\mathbf{RS}}} \right) = tr\left( {{\mathbf{RU\Sigma }}{{\mathbf{V}}^T}} \right) = tr\left( {{\mathbf{\Sigma }}{{\mathbf{V}}^T}{\mathbf{RU}}} \right)
\]
根据SVD分解性质，以及旋转矩阵的性质，\(\mathbf{V}\) 、\(\mathbf{R}\) 、\(\mathbf{U}\) 都是正交矩阵。若令 \({\mathbf{M}} = {{\mathbf{V}}^T}{\mathbf{RU}}\) ，则 \(\mathbf{M}\) 也是正交矩阵。
\[tr\left( {{\mathbf{\Sigma }}{{\mathbf{V}}^T}{\mathbf{RU}}} \right) = tr\left( {{\mathbf{\Sigma M}}} \right)
\]
对于正交矩阵 \(\mathbf{M}\) ，列向量 \(\mathbf{m}_j\) 也是正交的，故矩阵的每个元素 \(m_{ij}\) 都小于1：
\[1 = {\mathbf{m}}_j^T{{\mathbf{m}}_j} = \sum\limits_{i = 1}^d {m_{.ij}^2} \Rightarrow {m_{ij}} \leqslant 1 \Rightarrow \left| {{m_{ij}}} \right| < 1
\]
而 SVD 分解得到的 \({\mathbf{\Sigma }}\) 矩阵为对角矩阵，对角元素满足 \({\sigma _1},{\sigma _2}, \cdots ,{\sigma _d} \geqslant 0\) 。于是：
\[tr\left( {\mathbf{\Sigma} \mathbf{M}} \right) = \left( {\begin{array}{*{20}{c}}
{{\sigma _1}}&{}&{}&{} \\
{}&{{\sigma _2}}&{}&{} \\
{}&{}& \ddots &{} \\
{}&{}&{}&{{\sigma _d}}
\end{array}} \right)\left( {\begin{array}{*{20}{c}}
{{m_{11}}}&{{m_{12}}}& \cdots &{{m_{1d}}} \\
{{m_{21}}}&{{m_{22}}}& \cdots &{{m_{2d}}} \\
\vdots & \vdots & \vdots & \vdots \\
{{m_{d1}}}&{{m_{d2}}}& \cdots &{{m_{dd}}}
\end{array}} \right) = \sum\limits_{i = 1}^d {{\sigma _i}{m_{ii}}} \leqslant \sum\limits_{i = 1}^d {{\sigma _i}}
\]
所以当 \(m_{ii}=1\) 时，上式取得最大值。而 \(\mathbf{M}\) 为正交矩阵，故此时 \(\mathbf{M}\) 为一单位矩阵。
\[{\mathbf{I}} = {\mathbf{M}} = {{\mathbf{V}}^T}{\mathbf{RU}} \Rightarrow {\mathbf{V}} = {\mathbf{RU}} \Rightarrow {\mathbf{R}} = {\mathbf{V}\mathbf{U}^T}
\]
旋转矩阵校验
由于旋转矩阵和反射矩阵都是正交矩阵，而本问题的最终结果必须为旋转矩阵，因需要对结果进行校验。校验的依据就是矩阵的行列式。
1、若\(\det ({\mathbf{R}}) = \det ({\mathbf{V}}{{\mathbf{U}}^T}) = 1\)，则 该矩阵就是所需要求解的旋转矩阵，\({\mathbf{R}^*} = {\mathbf{V}\mathbf{U}^T}\)
2、若\(\det ({\mathbf{R}}) = \det ({\mathbf{V}}{{\mathbf{U}}^T}) = -1\) ，则该矩阵为一反射矩阵，需要基于此情况寻找一个局部最优解。
取 \(m_{dd} = -1, m_{ii}=1,i=1,2,\cdots,d-1\) ，则有：
\[{\mathbf{M}} = {{\mathbf{V}}^T}{\mathbf{RU}} = \left( {\begin{array}{*{20}{c}}
1&{}&{}&{} \\
{}&1&{}&{} \\
{}&{}& \ddots &{} \\
{}&{}&{}&{ - 1}
\end{array}} \right) \Rightarrow {\mathbf{R}} = {\mathbf{V}}\left( {\begin{array}{*{20}{c}}
1&{}&{}&{} \\
{}&1&{}&{} \\
{}&{}& \ddots &{} \\
{}&{}&{}&{ - 1}
\end{array}} \right){{\mathbf{U}}^T}
\]
将以上两种情况统一，则可将旋转矩阵表示为：
\[{\mathbf{R}} = {\mathbf{V}}\left( {\begin{array}{*{20}{c}}
1&{}&{}&{} \\
{}&1&{}&{} \\
{}&{}& \ddots &{} \\
{}&{}&{}&{\det ({\mathbf{V}}{{\mathbf{U}}^T})}
\end{array}} \right){{\mathbf{U}}^T}
\]
平移矩阵求解
根据前述推导，得到旋转矩阵后，平移矩阵可表示为：
\[{\mathbf{t}} = {\mathbf{\bar q}} - {\mathbf{R\bar p}}
\]
参考文献
