问题描述
已知在 \(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\) 时，函数有最小值，于是有
\[\begin{gathered}
0 = \sum\limits_{i = 1}^n {2{w_i}\left( {{\mathbf{R}}{{\mathbf{p}}_i} + {\mathbf{t}} - {{\mathbf{q}}_i}} \right)} \hfill \\
\;\; = 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}} \hfill \\
\end{gathered}
\]
令：
\[{\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{gathered}
\sum\limits_{i = 1}^n {{w_i}{{\left\| {({\mathbf{R}}{{\mathbf{p}}_i} + {\mathbf{t}}) - {{\mathbf{q}}_i}} \right\|}^2}} = \hfill \\
= \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}} \hfill \\
= \sum\limits_{i = 1}^n {{w_i}{{\left\| {{\mathbf{R}}({{\mathbf{p}}_i} - {\mathbf{\bar p}}) - ({{\mathbf{q}}_i} - {\mathbf{\bar q}})} \right\|}^2}} \hfill \\
\end{gathered}
\]
令：
\[{{\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.
\]
则目标函数可重新定义为：
\[\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{gathered}
{\left\| {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right\|^2} = \hfill \\
= {\left( {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right)^T}\left( {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right) \hfill \\
= \left( {{\mathbf{x}}_i^T{{\mathbf{R}}^T} - {\mathbf{y}}_i^T} \right)\left( {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right) \hfill \\
= {\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} \hfill \\
\end{gathered}
\]
其中，根据旋转矩阵的正交性性质可知，\({{\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{gathered}
\mathop {\arg \min }\limits_{\mathbf{R}} \sum\limits_{i = 1}^n {{w_i}{{\left\| {{\mathbf{R}}{{\mathbf{x}}_i} - {{\mathbf{y}}_i}} \right\|}^2}} = \hfill \\
= \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{gathered}
\]
参考文献
- Sorkine O . Least-squares rigid motion using svd. 2009.