[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-2\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-R\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<pair<Vec3_t, Vec3_t>>& 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<N; i++ )

    {

        p += match_pairs[i].first;  // \sum_{i=1}^{n} \mathbf{p}_{i}

        q += match_pairs[i].second; // \sum_{i=1}^{n} \mathbf{q}_{i}

    }

    p /= N; //\frac {\sum_{i=1}^{n} \mathbf{p}_{i}}{n}

    q /= N; //\frac {\sum_{i=1}^{n} \mathbf{1}_{i}}{n}

    //2. 计算\mathbf{x},\mathbf{y}

    vector<Vec3_t> X( N ), Y( N ); // remove the center

    for ( int i=0; i<N; i++ )

    {

        X[i] = match_pairs[i].first - p;

        Y[i] = match_pairs[i].second - q;

    }

    //3. S=XWY^{\top} W=E, 因此S=XY^{\top}

    Eigen::Matrix3d S = Eigen::Matrix3d::Zero();

    for ( int i=0; i<N; i++ )

    {

        S += X[i] * Y[i].transpose();

    }

    cout<<"S="<<S<<endl;

    //4. S进行SVD 奇异值分解

    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( S, Eigen::ComputeFullU|Eigen::ComputeFullV );

    const Eigen::Matrix3d U = svd.matrixU();

    const Eigen::Matrix3d V = svd.matrixV();

    cout<<"U="<<U<<endl;

    cout<<"V="<<V<<endl;

    //5. 构造去镜像矩阵

    Eigen::Matrix3d remove_mirror{Eigen::Matrix3d::Identity()};

    remove_mirror(2,2) = (V*U.transpose()).determinant();

    cout<<"remove_mirror="<<remove_mirror<<endl;

    //6. R=V*remove_mirror*U^{\top}

    R = V*remove_mirror*U.transpose();

    //7. 平移向量$\mathbf{t}=\overline{\mathbf{q}}-R \overline{\mathbf{p}}\tag{31}$

    t = p - R * q;

    cout<< "SVD method:"<<endl;

    cout<<"R="<<R<<endl;

    cout<<"t="<<t.transpose()<<endl;

}

参考

使用 SVD 方法求解 ICP 问题