基于奇异值分解的点云配准原理-尧图网络科技

问题描述

已知在 \(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\) 时，函数有最小值^[1]，于是有

\[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{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} \]