[ICP]手推SVD方法

该方法源于《Least-Squares Rigid Motion Using SVD》，原文推导十分详细，这里自己也仔细推导了一遍，有些地方加以注释整理。

问题定义

假设我们有两个点云集合\(\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\}\)，则我们定义的 ICP 问题是通过最小化点对之间距离获得相应的位姿\((R,\mathbf{t})\)：

\[(R, \mathbf{t})=\underset{R \in SO(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}\]
其中 \(w_i\) 代表每个点的权重。R 和 t 是我们所要求的旋转矩阵和平移向量。

计算平移向量

我们要优化的误差函数如下:
\(F(R,\mathbf{t})=\sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2}\)
首先来计算对t的导数
令\(\mathcal{l}=\left\|\left(R\mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2}=(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^T(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)\)
\(dl\)的微分为：

\[ \begin{aligned} dl &=d((R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^T)(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)+(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Td(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i) \\ &=\underbrace{(d(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i))^T}_{d(X^T) = (dX)^T}(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)+(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Td\mathbf{t} \\ &=(d\mathbf{t})^T(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)+(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Td\mathbf{t} \\ &=\underbrace{2(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)^Tdt}_{当A^TB为标量时，A^TB=B^TA} \end{aligned} \]
对照\(dl=\frac{\partial l}{\partial t}^Tdt\)，得\(\frac{\partial l}{\partial t}=2(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i)\)。因此:

\[ \begin{aligned} \frac{\partial F}{\partial t} &=\sum_{i=1}^{n} 2 w_{i}(R\mathbf{p}_i+\mathbf{t}-\mathbf{q}_i) \\ &=2\mathbf{t}\sum_{i=1}^{n}w_i+2R\sum_{i=1}^{n}w_i\mathbf{p}_i-2R\sum_{i=1}^{n}w_i\mathbf{q}_i\end{aligned} \tag{2} \]
令\(\frac{\partial F}{\partial t}=0\)，得

\[ \mathbf{t}=\frac {\sum_{i=1}^{n}w_i\mathbf{q}_i-\sum_{i=1}^{n}w_i\mathbf{p}_i}{\sum_{i=1}^{n}w_i} \]
记：

\[ \overline{\mathbf{p}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{p}_{i}}{\sum_{i=1}^{n} w_{i}}, \quad \overline{\mathbf{q}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{q}_{i}}{\sum_{i=1}^{n} w_{i}}\tag{3} \]
也就是加权平均的质心，并再次带回 (2) 式可以得到：
\(\mathbf{t}=\overline{\mathbf{q}}-R \overline{\mathbf{p}}\tag{4}\)

将(4)带回(1)可得：

\[ \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\left(\mathbf{p}_{i}-\overline{\mathbf{p}}\right)-\left(\mathbf{q}_{i}-\overline{\mathbf{q}}\right)\right\|^{2} \end{aligned}\tag{5} \]
令\(\mathbf{x}_i:=\mathbf{p}_i-\overline{\mathbf{p}},\mathbf{y}_i:=\mathbf{q}_i-\overline{\mathbf{q}}\)，则问题转变为：

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

计算旋转矩阵

(6)式是不是很眼熟，还记得最小二乘问题吗？

\[ \begin{aligned}\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2} &=\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right)^{\top}\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right)=\left(\mathbf{x}_{i}^{\top} R^{\top}-\mathbf{y}_{i}^{\top}\right)\left(R \mathbf{x}_{i}-\mathbf{y}_{i}\right) \\ &=\underbrace{\mathbf{x}_{i}^{\top}R^{\top}R\mathbf{x}_{i}}_{R是单位正交阵，R^{\top}R=\mathbf{I}}-\mathbf{y}_{i}^{\top} R \mathbf{x}_{i}-\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i} \\ &=\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-\mathbf{y}_{i}^{\top}R\mathbf{x}_{i}-\underbrace{(R\mathbf{x}_{i})^{\top}\mathbf{y}_{i}}_{(AB)^{\top}=B^{\top}A^{\top}}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i} \\ &=\mathbf{x}_{i}^{\top} \mathbf{x}_{i}\underbrace{-2\mathbf{y}_{i}^{\top}R\mathbf{x}_{i}}_{当A^TB为标量时，A^TB=B^TA} + \mathbf{y}_{i}^{\top}\mathbf{y}_{i} \end{aligned}\tag{7} \]
因此问题转换为：

\[ \begin{aligned}\underset{R\in SO(d)}{\operatorname{argmin}} \sum_{i=1}^{n}w_i\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2}&=\underset{R\in SO(d)}{\operatorname{argmin}} \:\sum_{i=1}^{n}w_i(\underbrace{\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2\mathbf{y}_{i}^{\top}R\mathbf{x}_i + \mathbf{y}_{i}^{\top}\mathbf{y}_{i}}_{\mathbf{x}_i,\mathbf{y}_i与R无关}) \\ &=\underset{R\in SO(d)}{\operatorname{argmax}} \:\sum_{i=1}^{n}w_i\mathbf{y}_{i}^{\top}R\mathbf{x}_i \end{aligned}\tag{8} \]
令\(W_{n\times n}=\left[\begin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \\ {} & {w_{2}} & {} & {} & {} \\ {} & {} & {} & {\ddots} & {} \\ {} & {} & {} & {} & {w_{n}}\end{array}\right],Y^{\top}_{n\times 3} = \left[\begin{array}{c}{-\mathbf{y}_{1}^{\top}-} \\ {-\mathbf{y}_{2}^{\top}-} \\ {\vdots} \\ {-\mathbf{y}_{n}^{\top}-}\end{array}\right],X_{3\times n}=\begin{bmatrix}| & | & & |\\\mathbf{x}_1 & \mathbf{x}_2 & \cdots & \mathbf{x}_n\\ | & | & & |\end{bmatrix}\)
有

\[ \begin{align*}W_{n\times n}Y^{\top}_{n\times 3} R_{3\times 3} X_{3\times n} &= \left[\begin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \\ {} & {w_{2}} & {} & {} & {} \\ {} & {} & {} & {\ddots} & {} \\ {} & {} & {} & {} & {w_{n}}\end{array}\right] \left[\begin{array}{c}{-\mathbf{y}_{1}^{\top}-} \\ {-\mathbf{y}_{2}^{\top}-} \\ {\vdots} \\ {-\mathbf{y}_{n}^{\top}-}\end{array}\right] \left[\begin{array}{ccccc}{} & {} & {} \\ {} & {R} & {} \\ {} & {} & {} \end{array}\right] \begin{bmatrix}| & | & & |\\\mathbf{x}_1 & \mathbf{x}_2 & \cdots & \mathbf{x}_n\\ | & | & & |\end{bmatrix}\\ &= \left[\begin{array}{c}{-w_{1}\mathbf{y}_{1}^{\top}-} \\ {-w_{2}\mathbf{y}_{2}^{\top}-} \\ {\vdots} \\ {-w_{n}\mathbf{y}_{n}^{\top}-}\end{array}\right]_{n\times 3} \begin{bmatrix}| & | & & |\\R\mathbf{x}_1 & R\mathbf{x}_2 & \cdots & R\mathbf{x}_n\\ | & | & & |\end{bmatrix}_{3\times n}\\ &= \left[\begin{array}{cccc}{w_{1} \mathbf{y}_{1}^{\top} R \mathbf{x}_{1}} & {} & {} & {*} \\ {} & {w_{2} \mathbf{y}_{2}^{\top} R \mathbf{x}_{2}} & {} \\ {} & {} & {\ddots} & {} \\ {*} & {} & {} & {w_{n} \mathbf{y}_{n}^{\top} R \mathbf{x}_{n}}\end{array}\right] \end{align*}\tag{9} \]
因此

\[ \begin{aligned}\sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}&=\operatorname{tr}\left(W Y^{\top} R X\right)\\ &=\underbrace{tr(RXWY^{\top})}_{tr(AB)=tr(BA)} \end{aligned}\tag{10}\]
令\(S=XWY^{\top}\)，而\(S_{SVD}=U\Sigma V^{\top},U与V都是单位正交阵，即UU^{\top}=I,VV^{\top}=I,\Sigma =\left(\begin{array}{cccc}{\sigma_{1}} & {} & {} & {} \\ {} & {\sigma_{2}} & {} & {} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {\sigma_{n}}\end{array}\right)且\sigma_{1}, \sigma_{2}, \ldots, \sigma_{n} \geq 0\)，带入(10):

\[ tr(RXWY^{\top})=tr(RS)=tr(RU\Sigma V^{\top})=tr(\Sigma V^{\top}RU) \tag{11} \]
我们来看下\(M=V^{\top}RU\)，\(V^{\top},R,U\)均为单位正交阵，那么\(M\)也为单位正交阵（自己动手推导下，很简单的～），有\(MM^{\top}=I\)，即M中每行、每列的内积都是1。假设\(\mathbf{m}_j为M的列向量\)，那么

\[\mathbf{m}_j^{\top}\mathbf{m}_j=\sum_{i}m_{ij}^2=1\]
可见\(\forall i,j\in[0,n],|m_{ij}|\leqslant 1\)。那么

\[ \operatorname{tr}(\Sigma M)=\left(\begin{array}{cccc}{\sigma_{1}} & {} & {} & {} \\ {} & {\sigma_{2}} & {} & {} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {\sigma_{n}}\end{array}\right)\left(\begin{array}{c}{m_{11} m_{12} \ldots m_{1 n}} \\ {m_{21} m_{22} \ldots m_{2 n}} \\ {\vdots} \\ {m_{n 1} m_{n 2} \ldots m_{n n}}\end{array}\right)=\sum_{i=1}^{n} \sigma_{i} m_{i i} \leq \sum_{i=1}^{n} \sigma_{i}\tag{12} \]
显然\(M=I\)时，\(tr(\Sigma M)\)可以取到最大值，此时

\[I=M=V^{\top} R U \Rightarrow V=R U \Rightarrow R=V U^{\top}\tag{13}\]

反射修正

前文中推导的结果一定是一个单位正交阵，但是有一个问题，并不是所有的单位正交阵都是旋转矩阵。

镜面反射

参考
\(A=\begin{bmatrix} \text{cos}\theta & \text{sin}\theta \\ \text{sin}\theta & -\text{cos}\theta\end{bmatrix}\)
也是一正交矩阵，仔细观察两个基的变化，它相当于逆时针旋转\(\theta\)后再把\(y'\) 轴对折，物理上若不对折，无论如何旋转也达不到依运算所得的结果，显然这类正交矩阵既包括旋转还包括了镜面反射。这里是二维的情况，对于三维同样有效，因此求解出R后还需要进行一些检测：
如果\(\operatorname{det}\left(V U^{\top}\right)=-1\)，则所求的 R 包含了旋转和镜像；
如果 \(\operatorname{det}\left(V U^{\top}\right)=1\)，则所求的 R 是我们所求的旋转矩阵。

假设包含了旋转和镜像，对于上节的结论：

\[ M=V^{\top}RU=\left(\begin{array}{ccccc}{1} \\ {} & {1} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {-1}\end{array}\right) \Rightarrow R=V\left(\begin{array}{ccccc}{1} \\ {} & {1} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {-1}\end{array}\right) U^{\top}\tag{13} \]
整理上述两种情况就可以统一成以下表达式：

\[ R=V \begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & \operatorname{det}\left(V U^{\top}\right)\end{pmatrix} U^{\top}\tag{14} \]
平移向量\(\mathbf{t}=\overline{\mathbf{q}}-R \overline{\mathbf{p}}\tag{31}\)

实践

void pose_estimation_svd (
        const vector>& match_pairs,
        Mat33_t& R, Vec3_t& t)
{
    //假设每个点的权重都是1.0
    // \overline{\mathbf{p}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{p}_{i}}{\sum_{i=1}^{n} w_{i}} = \frac {\sum_{i=1}^{n} \mathbf{p}_{i}}{n}
    // \overline{\mathbf{q}}=\frac{\sum_{i=1}^{n} w_{i} \mathbf{q}_{i}}{\sum_{i=1}^{n} w_{i}} = \frac {\sum_{i=1}^{n} \mathbf{q}_{i}}{n}

    //1. 计算\overline{\mathbf{p}}，\overline{\mathbf{q}}
    Vec3_t p{0.,0.,0.}, q{0.,0.,0.};
    int N = match_pairs.size();
    for ( int i=0; i X( N ), Y( N ); // remove the center
    for ( int i=0; i参考 
 
 
  
使用 SVD 方法求解 ICP 问题