svd求解点云旋转矩阵方法_使用 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}_