使用 SVD 方法求解 ICP 问题

本文是结合《Least-Squares Rigid Motion Using SVD》和《Least-Squares Fitting of Two 3-D Point Sets》两篇文章写的一个总结,里面有一些是自己的理解不一定正确。

1 问题定义

假设我们有两个点云集合 \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 问题是通过最小化点对之间距离获得相应的 Pose:

(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}

其中 w_i 代表每个点的权重。R 和 t 是我们所要求的旋转矩阵和平移向量。

2 计算平移

我们分两步求解旋转和平移,首先求解 t。固定 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 求导得到:

\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 \mathrm{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}}, \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}
那么可以知道,t 可以由加权的质心得到。再将 t 带回目标函数 F 可得:

\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}}, \quad \mathbf{y}_{i} :=\mathbf{q}_{i}-\overline{\mathbf{q}}\tag{6}
我们可以直接求质心后对每个点进行归一化再求解。有了 t 之后,我们要求解的 R 如下:

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}
其中,d 表示 x 和 y 的维度,通常情况下对于三维点云就是 d = 3,对于二维点云就是 d = 2。

3 计算旋转

首先简化下 (7) 式,先看权重后面的部分,则公式推导如下:

\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)=\\ &=\mathbf{x}_{i}^{\top} R^{\top} R \mathbf{x}_{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}-\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i} \end{aligned}\tag{8}
上述推导中最后一步解释下:

首先,R^{\top} R=I,因此有 \mathbf{x}_{i}^{\top} R^{\top} R \mathbf{x}_{i} = \mathbf{x}_{i}^{\top} \mathbf{x}_{i}

其次,由于 \mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i} 是一个标量(这是很显然的,\mathbf{x}_{i}^{\top}1 \times d 的向量,R^{\top}d \times d 矩阵,\mathbf{y}_{i}d \times 1 向量)。对于标量,我们有性质 a=a^{\top},因此有:

\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}=\left(\mathbf{x}_{i}^{\top} R^{\top} \mathbf{y}_{i}\right)^{\top}=\mathbf{y}_{i}^{\top} R \mathbf{x}_{i}\tag{9}
带回公式 (8) 可以得到:

\left\|R \mathbf{x}_{i}-\mathbf{y}_{i}\right\|^{2}=\mathbf{x}_{i}^{\top} \mathbf{x}_{i}-2 \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}+\mathbf{y}_{i}^{\top} \mathbf{y}_{i}\tag{10}
整理公式 (7) 可以得到:

\begin{aligned} & \operatorname{argmin}_{R \in S O(d)} \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)=\\=& \operatorname{argmin}_{R \in S O(d)}\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)=\\=& \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) \end{aligned}\tag{11}

最后一个的等号显而易见,因为 \sum_{i=1}^{n} w_{i} \mathbf{x}_{i}^{\top} \mathbf{x}_{i}\sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} \mathbf{y}_{i} 两项都与 R 无关。再进一步去掉负号改成求最大值则有:

\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}} \sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}\tag{12}
根据矩阵迹的性质,我们有以下结论:

\sum_{i=1}^{n} w_{i} \mathbf{y}_{i}^{\top} R \mathbf{x}_{i}=\operatorname{tr}\left(W Y^{\top} R X\right)\tag{13}
其中:W=\left[\begin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \\ {} & {w_{2}} & {} & {} & {} \\ {} & {} & {} & {\ddots} & {} \\ {} & {} & {} & {} & {w_{n}}\end{array}\right]Y^{\top} = \left[\begin{array}{c}{-\mathbf{y}_{1}^{\top}-} \\ {-\mathbf{y}_{2}^{\top}-} \\ {\vdots} \\ {-\mathbf{y}_{n}^{\top}-}\end{array}\right]X=\begin{bmatrix}| & | & & |\\x_1 & x_2 & \cdots & x_n\\ | & | & & |\end{bmatrix}

上述公式推导如下:

\begin{aligned}W Y^{\top} R X &= \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}| & | & & |\\x_1 & x_2 & \cdots & 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] \begin{bmatrix}| & | & & |\\Rx_1 & Rx_2 & \cdots & Rx_n\\ | & | & & |\end{bmatrix}\\ &= \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{aligned}\tag{14}

因此很明显可以得出公式 (13) 中的结论。

之所以用这样表示是因为用矩阵的迹有很多性质可以利用,根据迹的性质我们有对称性:

\operatorname{tr}(A B)=\operatorname{tr}(B A)\tag{15}
因此我们有:

\operatorname{tr}\left(W Y^{\top} R X\right)=\operatorname{tr}\left(\left(W Y^{\top}\right)(R X)\right)=\operatorname{tr}\left(R X W Y^{\top}\right)\tag{16}
定义“协方差”矩阵 S=X W Y^{\top},对 S 进行 SVD 分解:

S=U \Sigma V^{\top}\tag{17}
将上述公式带入 (16) 同时再根据性质 (15),则有:

\operatorname{tr}\left(R X W Y^{\top}\right)=\operatorname{tr}(R S)=\operatorname{tr}\left(R U \Sigma V^{\top}\right)=\operatorname{tr}\left(\Sigma V^{\top} R U\right)\tag{18}
因为其中 V、R 和 U 都是正交矩阵。因此 M=V^{\top} R U 也是正交矩阵。根据正交矩阵的定义其行、列向量均为正交的单位向量,因此对于矩阵 M 中每个列向量 \mathbf{m_j} 均有 \mathbf{m}_{j}^{\top} \mathbf{m}_{j}=1。可以推出 M 中每一个元素的绝对值均有 \left | \mathbf{m_{ij}}\right |\leqslant 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{19}
根据 SVD 分解的性质 \Sigma =\left(\begin{array}{cccc}{\sigma_{1}} & {} & {} & {} \\ {} & {\sigma_{2}} & {} & {} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {\sigma_{d}}\end{array}\right),其中对角线的元素 \sigma_{1}, \sigma_{2}, \ldots, \sigma_{d} \geq 0,因此有:

\operatorname{tr}(\Sigma M)=\left(\begin{array}{cccc}{\sigma_{1}} & {} & {} & {} \\ {} & {\sigma_{2}} & {} & {} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {\sigma_{d}}\end{array}\right)\left(\begin{array}{c}{m_{11} m_{12} \ldots m_{1 d}} \\ {m_{21} m_{22} \ldots m_{2 d}} \\ {\vdots} \\ {m_{d 1} m_{d 2} \ldots m_{d d}}\end{array}\right)=\sum_{i=1}^{d} \sigma_{i} m_{i i} \leq \sum_{i=1}^{d} \sigma_{i}\tag{20}
上式等号成立也就是取得最大值的唯一条件是每一个 \mathbf{m}_{i i}=1,由于 M 是正交矩阵,其行列向量均为正交的单位向量,因此只能是 M 为单位矩阵:M = I,得到:

I=M=V^{\top} R U \Rightarrow V=R U \Rightarrow R=V U^{\top}\tag{21}
这就是 ICP 中求解时计算 R=V U^{\top} 的推导过程。

4 反射修正

4.1 反射变换

首先理解下反射变换(也叫镜面反射),根据维基百科定义,在数学上反射是把一个物体变换成它的镜像的映射。要反射一个平面图形,需要“镜子”是一条直线(反射轴),对于三维空间中的反射就要使用平面作为镜子。

反射变换同样是一个正交矩阵,显而易见它满足如下性质:

  • 镜面反射是正交变换。
  • 镜面反射的逆变换为镜面反射。
  • 任意一个正交变换都可以表示成若干个镜面反射的乘积。

除此之外,旋转变换和反射变换具有如下特性(对于2D和3D同样成立):

  • 旋转矩阵和反射矩阵都是正交矩阵
  • 旋转矩阵的行列式值为 +1,反射矩阵的行列值为 -1
  • 旋转矩阵 R(\theta) 的逆矩阵为 R(-\theta),反射矩阵的逆矩阵为其本身。或者也可以记为,旋转矩阵:R^{\top}R=I,反射矩阵:R'R'=I
  • 旋转矩阵和反射矩阵可以相互转换:
    \begin{aligned}\operatorname{Ref}(\theta) \operatorname{Ref}(\phi) &=\operatorname{Rot}(2(\theta-\phi)) \\ \operatorname{Rot}(\theta) \operatorname{Rot}(\phi) &=\operatorname{Rot}(\theta+\phi) \\ \operatorname{Rot}(\theta) \operatorname{Ref}(\phi) &=\operatorname{Ref}(\phi+\theta / 2) \\ \operatorname{Ref}(\phi) \operatorname{Rot}(\theta) &=\operatorname{Ref}(\phi-\theta / 2) \end{aligned}\tag{22}

最简单的反射变换是沿某个轴/面的镜像,例如相对于 Z=0 平面的镜像变换为:\begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & -1\end{pmatrix}。则根据上面的最后一条性质,我们一定可以把任意一个镜面变换拆成一个关于 Z=0 的镜面变换与一个旋转变换。也就是变成:

R'=\begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & -1\end{pmatrix}R\tag{23}

4.2 反射修正

通过之前的推导,我们用 SVD 求解的 R 一定是一个正交矩阵,但是并不是所有的正交矩阵都是旋转矩阵,也可能是一个反射矩阵(或者说包含了反射变换的矩阵)。因此我们还需要对所求得的 R 进行行列式判断,判断方法也很简单:

  • 如果 \operatorname{det}\left(V U^{\top}\right)=-1,则所求的 R 包含了镜像;
  • 如果 \operatorname{det}\left(V U^{\top}\right)=1,则所求的 R 是我们所求的旋转矩阵。

这一部分在《Least-Squares Fitting of Two 3-D Point Sets》里面讲得更详细一点。假设 \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\} 是可以完美重合的两团点云,两个点云有如下三种情况需要讨论:

1)点云 \mathcal{Q} 不共面

在这种情况下,显然有唯一的旋转矩阵 R 满足公式 (1)。可以直观想象不共面的两个点云(当然也需要点云本身不满足镜像性质)是不可能通过镜像变换映射成自身的。

2)点云 \mathcal{Q} 共面不共线

在这种情况下,满足公式 (1) 的解不唯一。一定同时存在旋转矩阵 R 和反射矩阵 R' 都可以满足。说明如下:

对于 3x3 协方差矩阵 S=X W Y^{\top} 由于点 X 和 Y 是共面的,也就意味着存在平面法向 n = {n_x, n_y, n_z} 满足 nX = d,那么 X 和 Y 均只有两个自由变量,S 的秩为 2。那么也就是:

S=U \Sigma V^{\top}=U \begin{pmatrix}\sigma_1 & 0 & 0\\ 0 & \sigma_2 & 0\\ 0 & 0 & \sigma_2\end{pmatrix} V^{\top}\tag{24}
由于 SVD 分解特征值是从大到小排序,则一定有:

S=\sigma_{1} u_{1} v_{1}^{\top}+\sigma_{2} u_{2} v_{2}^{\top}+\sigma_{3} u_{3} v_{3}^{\top}=\sigma_{1} u_{1} v_{1}^{\top}+\sigma_{2} u_{2} v_{2}^{\top}+0 \cdot u_{3} v_{3}^{\top},\\ \sigma_{1}>\sigma_{2}>\sigma_{3}=0\tag{25}
如果存在一个解 R=V U^{\top}=\left [ v_1, v_2, v_3 \right ] U^{\top} 满足以上 S 取得极大值,则一定有镜像变换:

R’=V’ U^{\top}=\left [ v_1, v_2, -v_3 \right ] U^{\top}=\left [ v_1, v_2, v_3 \right ] \begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & -1\end{pmatrix}U^{\top}\tag{26}
满足上式 S 取得极大值。

所以当 \operatorname{det}\left(V U^{\top}\right)=-1 时,我们想要求得的 R 只需要去除中间乘的反射变换矩阵 \begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & -1\end{pmatrix} 即可。也就是 R=V U^{\top}=V' \begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & -1\end{pmatrix} U^{\top}

如果是 \operatorname{det}\left(V U^{\top}\right)=1 时,那么本身所求的就是旋转矩阵 R 我们也可以写成 R=V U^{\top}=V \begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\end{pmatrix} U^{\top}

整理上述两种情况就可以统一成以下表达式:

R=V U^{\top}=V \begin{pmatrix}1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & \operatorname{det}\left(V U^{\top}\right)\end{pmatrix} T^{\top}\tag{27}

3)点云 \mathcal{Q} 共线

这种情况下存在不止一种解,无法用 SVD 求得旋转矩阵。

5 总结

经过上面的推导和镜像修正,我们可以总结出一套完整的使用 SVD 求解 ICP 问题的流程:

我们的问题是求解 R, t 使得下面的误差函数最小:

\sum_{i=1}^{n} w_{i}\left\|\left(R \mathbf{p}_{i}+\mathbf{t}\right)-\mathbf{q}_{i}\right\|^{2}\tag{28}
则进行如下步骤:

1)计算点云中心:

\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{29}
2)对所有点做归一化:

\mathbf{x}_{i} :=\mathbf{p}_{i}-\overline{\mathbf{p}}, \quad \mathbf{y}_{i} :=\mathbf{q}_{i}-\overline{\mathbf{q}}, \quad i=1,2, \ldots, n\tag{30}
3)计算 d \times d 协方差矩阵(d代表数据维度):

S=X W Y^{\top}\tag{31}

其中:X=\begin{bmatrix}| & | & & |\\x_1 & x_2 & \cdots & x_n\\ | & | & & |\end{bmatrix}Y=\begin{bmatrix}| & | & & |\\y_1 & y_2 & \cdots & y_n\\ | & | & & |\end{bmatrix} 均是由 d 维列向量组成的 d \times n 维的矩阵;W=\left[\begin{array}{ccccc}{w_{1}} & {} & {} & {} & {} \\ {} & {w_{2}} & {} & {} & {} \\ {} & {} & {} & {\ddots} & {} \\ {} & {} & {} & {} & {w_{n}}\end{array}\right] 是权重矩阵。

4)计算 S 的 SVD 分解 S=U \Sigma V^{\top},则得到想要求的旋转矩阵 R 如下:

R=V\left(\begin{array}{ccccc}{1} \\ {} & {1} \\ {} & {} & {\ddots} & {} \\ {} & {} & {} & {\operatorname{det}\left(V U^{T}\right)}\end{array}\right) U^{\top}\tag{32}
5)计算平移向量 t:

\mathbf{t}=\overline{\mathbf{q}}-R \overline{\mathbf{p}}\tag{33}

参考文献

  1. https://blog.csdn.net/icameling/article/details/84103891
  2. https://blog.csdn.net/dfdfdsfdfdfdf/article/details/53213240
  3. https://blog.csdn.net/kfqcome/article/details/9356819https://blog.csdn.net/kfqcome/article/details/9356819
  4. https://math.stackexchange.com/questions/68119/why-does-ata-i-det-a-1-mean-a-is-a-rotation-matrix
  5. https://wangliangster.github.io/#/math/algra/rotatemat
  6. https://zhuanlan.zhihu.com/p/51362089

相关材料

原文 PDF:


4 Comments

Add a Comment

您的电子邮箱地址不会被公开。 必填项已用 * 标注