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

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

问题描述

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

$$\begin{array}{}\text{(1)}& (R,\mathbf{t})=\underset{R\in SO(d),\mathbf{t}\in {\mathbb{R}}^d}{\mathrm{argmin}}\sum _{i=1}^n{w}_i{\Vert (R{\mathbf{p}}_i+\mathbf{t})-{\mathbf{q}}_i\Vert }^2\end{array}$$

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

计算平移量

此时假定旋转矩阵$R$是固定的，令$$\begin{array}{}\text{(3)}& F(\mathbf{t})=\sum _{i=1}^n{w}_i{\Vert (R{\mathbf{p}}_i+\mathbf{t})-{\mathbf{q}}_i\Vert }^2\end{array}$$，我们可以通过$F$对$\mathbf{t}$求导的方式得到平移量的最优解，如下： $$\begin{array}{}\text{(2)}& \begin{array}{rl}0& =\frac{\partial F}{\partial \mathbf{t}}=\sum _{i=1}^{n}2w_i(R{\mathbf{p}}_i+\mathbf{t}-{\mathbf{q}}_i)=\\ & =2\mathbf{t}\left(\sum _{i=1}^n{w}_i\right)+2R\left(\sum _{i=1}^n{w}_i{\mathbf{p}}_i\right)-2\sum _{i=1}^n{w}_i{\mathbf{q}}_i\end{array}\end{array}$$

令： $$\begin{array}{}\text{(3)}& \bar{\mathbf{p}}=\frac{\sum _{i=1}^n{w}_i{\mathbf{p}}_i}{\sum _{i=1}^n{w}_i},\bar{\mathbf{q}}=\frac{\sum _{i=1}^n{w}_i{\mathbf{q}}_i}{\sum _{i=1}^n{w}_i}\end{array}$$

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

$$\begin{array}{}\text{(4)}& \mathbf{t}=\bar{\mathbf{q}}-R\bar{\mathbf{p}}\end{array}$$

从上式看出最优的平移量$\mathbf{t}$将$\mathcal{P}$点集的加权中心映射到了$\mathcal{Q}$点集的中心。接下来将上式带入优化方程，得： $$\begin{array}{}\text{(5)}& \begin{array}{rl}\sum _{i=1}^n{w}_i{\Vert (R{\mathbf{p}}_i+\mathbf{t})-{\mathbf{q}}_i\Vert }^2& =\sum _{i=1}^n{w}_i{\Vert R{\mathbf{p}}_i+\bar{\mathbf{q}}-R\bar{\mathbf{p}}-{\mathbf{q}}_i\Vert }^2\\ & =\sum _{i=1}^n{w}_i{\Vert R({\mathbf{p}}_i-\bar{\mathbf{p}})-({\mathbf{q}}_i-\bar{\mathbf{q}})\Vert }^2\end{array}\end{array}$$

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

$$\begin{array}{}\text{(6)}& {\mathbf{x}}_i:={\mathbf{p}}_i-\bar{\mathbf{p}}，{\mathbf{y}}_i:={\mathbf{q}}_i-\bar{\mathbf{q}}，\end{array}$$ 我们把问题简写成如下形式：

$$\begin{array}{}\text{(7)}& R=\underset{R\in SO(d)}{\mathrm{argmin}}\sum _{i=1}^n{w}_i{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2\end{array}$$

计算旋转量

简化上式： $$\begin{array}{}\text{(8)}& \begin{array}{rl}{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2& ={(R{\mathbf{x}}_i-{\mathbf{y}}_i)}^T(R{\mathbf{x}}_i-{\mathbf{y}}_i)=({\mathbf{x}}_i^T{R}^T-{\mathbf{y}}_i^T)(R{\mathbf{x}}_i-{\mathbf{y}}_i)\\ & ={\mathbf{x}}_i^T{R}^{T}R{\mathbf{x}}_i-{\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i-{\mathbf{y}}_i^{T}R{\mathbf{x}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i\end{array}\end{array}$$ 又因为旋转矩阵的正交性：$R^{T}R=I$；另外${\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i$是标量：${\mathbf{x}}_i$维度为$1\times d$，$R^T$维度为$d\times d$，${\mathbf{y}}_i$维度为$d\times 1$。于是有下式： $$\begin{array}{}\text{(9)}& {\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i=({\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i{)}^T={\mathbf{y}}_i^{T}R{\mathbf{x}}_i\end{array}$$ 得： $$\begin{array}{}\text{(10)}& {\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2={\mathbf{x}}_i^T{\mathbf{x}}_i-2{\mathbf{y}}_i^{T}R{\mathbf{x}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i\end{array}$$

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

$$\begin{array}{}\text{(11)}& \begin{array}{rl}& \underset{R\in SO(d)}{\mathrm{argmin}}\sum _{i=1}^n{w}_i{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2=\underset{R\in SO(d)}{\mathrm{argmin}}\sum _{i=1}^n{w}_i({\mathbf{x}}_i^{\mathrm{\top }}{\mathbf{x}}_i-2{\mathbf{y}}_i^{\mathrm{\top }}R{\mathbf{x}}_i+{\mathbf{y}}_i^{\mathrm{\top }}{\mathbf{y}}_i)=\\ =& \underset{R\in SO(d)}{\mathrm{argmin}}(\sum _{i=1}^n{w}_i{\mathbf{x}}_i^{\mathrm{\top }}{\mathbf{x}}_i-2\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{\mathrm{\top }}R{\mathbf{x}}_i+\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{\mathrm{\top }}{\mathbf{y}}_i)=\\ =& {\mathrm{argmin}}_{R\in SO(d)}(-2\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{\mathrm{\top }}R{\mathbf{x}}_i)\end{array}\end{array}$$ 接下来将要利用到如下关于迹的技巧:

$$\begin{array}{}\text{(4)}& \begin{array}{r}\left[\begin{array}{c}{w}_1\\ & w_1& & \\ & & \ddots & \\ & & & w_n\end{array}\right]\left[\begin{array}{ccc}—& {\mathbf{y}}_1^T& —\\ —& {\mathbf{y}}_2^T& —\\ —& ⋮& —\\ —& {\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& —\\ —& ⋮& —\\ —& 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^{T}R{\mathbf{x}}_1& & & \ast \\ & w_2{\mathbf{y}}_2^{T}R{\mathbf{x}}_2& & \\ & & \ddots & \\ \ast & & & w_n{\mathbf{y}}_n^{T}R{\mathbf{x}}_n\end{array}\right]\end{array}\end{array}$$

上式就是对$$\begin{array}{}\text{(5)}& \sum _{i=1}^n{w}_i{\mathbf{y}}_i^{\mathrm{\top }}R{\mathbf{x}}_i=\mathrm{tr}\left(W{Y}^{T}RX\right)\end{array}$$的完美解释。

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

$$\begin{array}{}\text{(12)}& \begin{array}{rl}\underset{R\in SO(d)}{\mathrm{argmin}}(-2\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{\mathrm{\top }}R{\mathbf{x}}_i)& =\underset{R\in SO(d)}{\mathrm{argmax}}\left(\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{\mathrm{\top }}R{\mathbf{x}}_i\right)\\ & =\underset{R\in SO(d)}{\mathrm{argmax}}\mathrm{tr}\left(W{Y}^{T}RX\right)\end{array}\end{array}$$ 这里说明一下维度：$W=diag(w_1,w_2,...,w_n)$维度为$n\times n$，$Y^T$维度为$n\times d$，$R$维度为$d\times d$，$X$维度为$d\times n$。

接下来回顾一下迹的性质：$\mathrm{tr}(AB)=\mathrm{tr}(BA)$，因此有下式： $$\begin{array}{}\text{(13)}& \mathrm{tr}\left(W{Y}^{T}RX\right)=\mathrm{tr}((W{Y}^T)(RX))=\mathrm{tr}\left(RXW{Y}^T\right)\end{array}$$

令$d\times d$的“covariance”矩阵$S=XW{Y}^T$，求$S$的SVD分解：

$$\begin{array}{}\text{(14)}& S=U\mathrm{\Sigma }{V}^T.\end{array}$$

于是式$(13)$变为：

$$\begin{array}{}\text{(15)}& \mathrm{tr}\left(W{Y}^{T}RX\right)=\mathrm{tr}\left(RS\right)=\mathrm{tr}\left(RU\mathrm{\Sigma }{V}^T\right)=\mathrm{tr}\left(\mathrm{\Sigma }{V}^{T}RU\right)\end{array}$$

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

$$\begin{array}{}\text{(16)}& 1={\mathbf{m}}_j^{\mathrm{\top }}{\mathbf{m}}_j=\sum _{i=1}^d{m}_{ij}^2\Rightarrow m_{ij}^2\le 1\Rightarrow \left|m_{ij}\right|\le 1\end{array}$$

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

$$\begin{array}{}\text{(17)}& \mathrm{tr}(\mathrm{\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_{1d}\\ m_{21}& m_{22}& \dots & m_{2d}\\ ⋮& ⋮& ⋮& ⋮\\ m_{d1}& m_{d2}& \dots & m_{dd}\end{array}\right)=\sum _{i=1}^d{\sigma }_i{m}_{ii}\le \sum _{i=1}^d{\sigma }_i\end{array}$$

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

$$\begin{array}{}\text{(18)}& I=M=V^{T}RU\Rightarrow R=V{U}^T\end{array}$$

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

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

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

可以总结的套路

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

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

$$\begin{array}{}\text{(6)}& \bar{\mathbf{p}}=\frac{\sum _{i=1}^n{w}_i{\mathbf{p}}_i}{\sum _{i=1}^n{w}_i},\bar{\mathbf{q}}=\frac{\sum _{i=1}^n{w}_i{\mathbf{q}}_i}{\sum _{i=1}^n{w}_i}\end{array}$$

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

$$\begin{array}{}\text{(7)}& {\mathbf{x}}_i:={\mathbf{p}}_i-\bar{\mathbf{p}}，{\mathbf{y}}_i:={\mathbf{q}}_i-\bar{\mathbf{q}},i=1,2...n\end{array}$$

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

$$\begin{array}{}\text{(8)}& S=XW{Y}^T\end{array}$$

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

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

$$\begin{array}{}\text{(9)}& R=V\left(\begin{array}{c}1\\ & \ddots & & \\ & & 1& \\ & & & \det \left(V{U}^{\mathrm{\top }}\right)\end{array}\right)U^{\mathrm{\top }}\end{array}$$

step5. 计算平移量：

$$\begin{array}{}\text{(10)}& \mathbf{t}=\bar{\mathbf{q}}-R\bar{\mathbf{p}}\end{array}$$