📝笔记：SLAM常见问题(四)：求解ICP，利用SVD分解得到旋转矩阵

今天讲一篇关于利用SVD方法求解ICP问题的文献《Least-Squares Rigid Motion Using SVD》，这篇文章非常精彩地推导出将\(3D\)点对齐问题的解析解，同时总结了求解该问题的统一范式。

问题描述

已知\[{\mathcal{P}=\left\{\mathbf{p}_{1}, \mathbf{p}_{2}, \ldots, \mathbf{p}_{n}\right\}}\]以及\[{\mathcal{Q}=\left\{\mathbf{q}_{1}, \mathbf{q}_{2}, \ldots, \mathbf{q}_{n}\right\}}\]是空间中（文中说的更加普适，\(\mathbf{p}_i , \mathbf{q}_i \in \mathbb{R}^{d}\)，可以表示\(d\)维空间）的匹配点集，我们试图找到这样的旋转矩阵\(R\)和平移向量\(\mathbf{t}\)最小化如下对齐误差（即ICP问题的形式）：

\[ (R, \mathbf{t})=\underset{R \in S O(d), \mathbf{t} \in \mathbb{R}^{d}}{\operatorname{argmin}} \sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2} \tag{1} \]

接下来文章分别推导了平移向量\(\mathbf{t}\)以及旋转矩阵\(R\)的解析解。

计算平移量

此时假定旋转矩阵\(R\)是固定的，令\[F(\mathbf{t}) = \sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2}\]，我们可以通过\(F\)对\(\mathbf{t}\)求导的方式得到平移量的最优解，如下： \[ \begin{aligned} 0 &=\frac{\partial F}{\partial \mathbf{t}}=\sum_{i=1}^{n} 2 w_{i}\left(R \mathbf{p}_{i}+\mathbf{t}-\mathbf{q}_{i}\right)=\\ &=2 \mathbf{t}\left(\sum_{i=1}^{n} w_{i}\right)+2 R\left(\sum_{i=1}^{n} w_{i} \mathbf{p}_{i}\right)-2 \sum_{i=1}^{n} w_{i} \mathbf{q}_{i} \end{aligned} \tag{2} \]

令： \[ \overline{\mathbf{p}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{p}_{i}}{\sum_{i=1}^{n} w_{i}}, \overline{\mathbf{q}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{q}_{i}}{\sum_{i=1}^{n} w_{i}} \tag{3} \]

于是我们得到\(\mathbf{t}\)的解：

\[ \mathbf{t} = \overline{\mathbf{q}} - R\overline{\mathbf{p}} \tag{4} \]

从上式看出最优的平移量\(\mathbf{t}\)将\(\mathcal{P}\)点集的加权中心映射到了\(\mathcal{Q}\)点集的中心。接下来将上式带入优化方程，得： \[ \begin{aligned} \sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2} &= \sum_{i=1}^{n} w_{i}\left\| R \mathbf{p}_{i}+ \overline{\mathbf{q}} - R\overline{\mathbf{p}} -\mathbf{q}_{i}\right\|^{2} \\ &= \sum_{i=1}^{n} w_{i}\left\|R (\mathbf{p}_{i} -\overline{\mathbf{p}}) - (\mathbf{q}_{i} - \overline{\mathbf{q}} ) \right\|^{2} \end{aligned} \tag{5} \]

由此我们将原问题转换成了无平移量的优化问题，令：

\[ \mathbf{x}_i := \mathbf{p}_{i} -\overline{\mathbf{p}}， \mathbf{y}_i := \mathbf{q}_{i} -\overline{\mathbf{q}}，\tag{6} \] 我们把问题简写成如下形式：

\[ R = \underset{R \in S O(d)}{\operatorname{argmin}} \sum_{i=1}^{n} w_{i}\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} \tag{7} \]

计算旋转量

简化上式： \[ \begin{aligned} \left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} &= \left( R\mathbf{x}_i - \mathbf{y}_i\right)^T\left( R\mathbf{x}_i - \mathbf{y}_i\right) = \left( \mathbf{x}_i^TR^T - \mathbf{y}_i^T\right)\left( R\mathbf{x}_i - \mathbf{y}_i\right) \\ &= \mathbf{x}_i^TR^TR\mathbf{x}_i - \mathbf{x}_i^TR^T\mathbf{y}_i - \mathbf{y}_i^TR\mathbf{x}_i +\mathbf{y}_i^T\mathbf{y}_i \end{aligned}\tag{8} \] 又因为旋转矩阵的正交性：\(R^TR=I\)；另外\(\mathbf{x}_i^TR^T\mathbf{y}_i\)是标量：\(\mathbf{x}_i\)维度为\(1 \times d\)，\(R^T\)维度为\(d \times d\)，\(\mathbf{y}_i\)维度为\(d \times 1\)。于是有下式： \[ \mathbf{x}_i^TR^T\mathbf{y}_i = (\mathbf{x}_i^TR^T\mathbf{y}_i)^T = \mathbf{y}_i^TR\mathbf{x}_i \tag{9} \] 得： \[ \left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} = \mathbf{x}_i^T\mathbf{x}_i - 2\mathbf{y}_i^TR\mathbf{x}_i +\mathbf{y}_i^T\mathbf{y}_i \tag{10} \]

将整理好的上式带入简化后的\(R\)优化问题，得：

\[ \begin{aligned} & \underset{R \in S O(d)}{\operatorname{argmin}} \sum_{i=1}^{n} w_{i}\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2}=\underset{R \in S O(d)}{\operatorname{argmin}} \sum_{i=1}^{n} w_{i}\left(\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2 \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i}\right)=\\=& \underset{R \in S O(d)}{\operatorname{argmin}}\left(\sum_{i=1}^{n} w_{i} \mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2 \sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}+\sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} \mathbf{y}_{i}\right)=\\=& \operatorname{argmin}_{R \in S O(d)}\left(-2 \sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}\right) \end{aligned}\tag{11} \] 接下来将要利用到如下关于迹的技巧:

\[ \begin{aligned} \left[ \begin{array} {cccc}{w_1} \\ {} & {w_1} & {} &{} \\ {} & {} & {\ddots} & {}\\ {} & {} & {} & {w_n} \end{array} \right] \left[ \begin{array} {ccc} {—}& {\mathbf{y}_1^T}&{—} \\ {—}& {\mathbf{y}_2^T}&{—} \\ {—} & {\vdots} & {—}\\ {—}& {\mathbf{y}_n^T}&{—} \end{array} \right] \left[ \begin{array} {ccc} {}& {} &{} \\ {}& {R} &{} \\ {}& {} &{} \\ \end{array} \right] \left[ \begin{array} {cccc} {|}& {|} &{|} &{|} \\ {\mathbf{x}_1}& {\mathbf{x}_2} &{\dots} &{\mathbf{x}_n} \\ {|}& {|} &{|} &{|} \\ \end{array} \right] \\ = \left[ \begin{array} {ccc} {—}& {w_1\mathbf{y}_1^T}&{—} \\ {—}& {w_2\mathbf{y}_2^T}&{—} \\ {—} & {\vdots} & {—}\\ {—}& {w_n\mathbf{y}_n^T}&{—} \end{array} \right] \left[ \begin{array} {cccc} {|}& {|} &{|} &{|} \\ {R\mathbf{x}_1}& {R\mathbf{x}_2} &{\dots} &{R\mathbf{x}_n} \\ {|}& {|} &{|} &{|} \\ \end{array} \right] \\ = \left[ \begin{array} {cccc} {w_1\mathbf{y}_1^TR\mathbf{x}_1}& {} &{} &{*} \\ {}& {w_2\mathbf{y}_2^TR\mathbf{x}_2} &{} &{} \\ {}& {} &{\ddots} &{} \\ {*}& {} &{} &{w_n\mathbf{y}_n^TR\mathbf{x}_n} \\ \end{array} \right] \end{aligned} \]

上式就是对\[\sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i} = \operatorname{tr}\left( WY^TRX\right)\]的完美解释。

利用上式，式\((11)\)可以整理得：

\[ \begin{aligned} \underset{R \in S O(d)}{\operatorname{argmin}}\left(-2 \sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}\right) &= \underset{R \in S O(d)}{\operatorname{argmax}}\left(\sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}\right) \\ &= \underset{R \in S O(d)}{\operatorname{argmax}} \operatorname{tr}\left( WY^TRX\right) \end{aligned}\tag{12} \] 这里说明一下维度：\(W = diag(w_1,w_2,...,w_n)\)维度为\(n \times n\)，\(Y^T\)维度为\(n \times d\)，\(R\)维度为\(d \times d\)，\(X\)维度为\(d \times n\)。

接下来回顾一下迹的性质：\(\operatorname{tr}(AB) = \operatorname{tr}(BA)\)，因此有下式： \[ \operatorname{tr}\left( WY^TRX\right) = \operatorname{tr}\left( (WY^T)(RX)\right) =\operatorname{tr}\left( RXWY^T\right) \tag{13} \]

令\(d\times d\)的“covariance”矩阵\(S = XWY^T\)，求\(S\)的SVD分解：

\[ S= U\Sigma V^T.\tag{14} \]

于是式\((13)\)变为：

\[ \operatorname{tr}\left( WY^TRX\right) =\operatorname{tr}\left( RS\right) =\operatorname{tr}\left( RU\Sigma V^T\right)=\operatorname{tr}\left( \Sigma V^TRU\right) \tag{15} \]

由于\(V,T,R\)均为正交矩阵，因此\(M = V^TRU\)也是正交阵，也就是说\(M\)的列向量\(\mathbf{m}_j\)是互相正交的单位向量，即\(\mathbf{m}_j^T\mathbf{m}_j=1\)，于是：

\[ 1=\mathbf{m}_{j}^{\top} \mathbf{m}_{j}=\sum_{i=1}^{d} m_{i j}^{2} \Rightarrow m_{i j}^{2} \leq 1 \Rightarrow\left|m_{i j}\right| \leq 1 \tag{16} \]

由于SVD分解的性质可知\(\sigma\)的元素均为非负数：\({\sigma}_1,{\sigma}_2,{\sigma}_d \geq 0\)，于是式\((18)\)变为如下形式：

\[ \operatorname{tr}(\Sigma M)=\left(\begin{array}{ccccc}{\sigma_{1}} & {} & {} & {} & {} \\ {} & {\sigma_{2}} & {} & {} & {} \\ {} & {} & {\ddots} & {} & {} \\ {} & {} & {} & {} & {\sigma_{d}}\end{array}\right)\left(\begin{array}{cccc}{m_{11}} & {m_{12}} & {\dots} & {m_{1 d}} \\ {m_{21}} & {m_{22}} & {\dots} & {m_{2 d}} \\ {\vdots} & {\vdots} & {\vdots} & {\vdots} \\ {m_{d 1}} & {m_{d 2}} & {\dots} & {m_{d d}}\end{array}\right) =\sum_{i=1}^{d} \sigma_{i} m_{i i} \leq \sum_{i=1}^{d} \sigma_{i} \tag{17} \]

可见，当迹最大时\(m_{ii} = 1\)，又由于\(M\)是正交阵，这使得\(M\)为单位阵！

\[I = M = V^TRU \Rightarrow R = VU^T \tag{18}\]

看到没，R的解析解竟然如此简单，并且与SVD分解产生了联系，让人感觉到了数学的美妙。不过到这里还没完，后面作者进行了一步方向矫正，大意是这样的：利用公式\((18)\)得到的矩阵并不一定是一个旋转矩阵，也可能为反射矩阵，此时可以通过验证\(VU^T\)的行列式来判断到底是旋转（行列式 = 1）还是反射（行列式 = -1）。但我们要求的是旋转矩阵，这时需要对公式\((18)\)进行一步处理。

假设\(\operatorname{det}(VU^T) = -1\)，则限制\(R\)为旋转就意味着\(M = V^TRU\)为反射矩阵， 于是我们试图找到一个反射矩阵\(M\)最大化下式： \[ \operatorname{tr}(\Sigma M) = {\sigma}_1 m_{11} + {\sigma}_2 m_{22} +...+ {\sigma}_d m_{dd} := f(m_{11},m_{11},...,m_{dd}) \tag{19} \] 即\(f\)是以\(m_{11},m_{11},...,m_{dd}\)为变量的线性函数，由于\(m_{ii} \in \left[ -1,1\right]\)，其极大值肯定在其定义域的边界处。于是当\({\forall} i, m_{ii} = 1\)时，\(f\)取得极大值，但是此时的\(R\)为反射矩阵，所以并不能这样取值。然后我们看第二个极大值点\((1,1,...,-1)\)，有： \[ f = \operatorname{tr}(\Sigma M) = {\sigma}_1 + {\sigma}_2+...+ {\sigma}_{d-1} - {\sigma}_d \tag{20} \] 这个值大于任何其它的自变量取值\((\pm 1,\pm 1,...,\pm 1)\)的组合（除了\(( 1, 1,..., 1)\)），因为奇异值是经过排序的，\({\sigma}_d\)是最小的一个奇异值。

综上，为了将解转换为旋转矩阵要进行如下处理： \[ R=V\left(\begin{array}{cccc}{1} \\ {} & {\ddots} & {} &{} \\ {} & {} & 1 & {}\\ {} & {} & {} & {\operatorname{det}\left(V U^{\top}\right)}\end{array}\right) U^{\top} \tag{21} \]

可以总结的套路

为了得到ICP问题的最优解，我们可以采取如下套路：

step1. 计算两组匹配点的加权中心：

\[ \overline{\mathbf{p}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{p}_{i}}{\sum_{i=1}^{n} w_{i}}, \overline{\mathbf{q}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{q}_{i}}{\sum_{i=1}^{n} w_{i}} \]

step2. 得到去中心化的点集：

\[ \mathbf{x}_i := \mathbf{p}_{i} -\overline{\mathbf{p}}， \mathbf{y}_i := \mathbf{q}_{i} -\overline{\mathbf{q}}, i = 1,2...n \]

step3. 计算\(d \times d\)的covariance矩阵：

\[ S = XWY^T \]

其中，\(X,Y\)为\(d \times n\)的矩阵，\(\mathbf{x}_i,\mathbf{y}_i\)分别是它们的列元素，另外\(W = diag(w_1,w_2,...,w_n)\)。

step4. 对\(S\)进行SVD分解\(S = U\Sigma V^T\)，得到旋转矩阵：

\[ R=V\left(\begin{array}{cccc}{1} \\ {} & {\ddots} & {} &{} \\ {} & {} & 1 & {}\\ {} & {} & {} & {\operatorname{det}\left(V U^{\top}\right)}\end{array}\right) U^{\top} \]

step5. 计算平移量：

\[ \mathbf{t} = \overline{\mathbf{q}} - R\overline{\mathbf{p}} \]