点云粗配准

点云精细配准受到初始值的影响，会收敛到局部最小值，因此点云的粗配准就变得相当重要。对于刚体来说，假设目前有原始点云 $\boldsymbol p_i^s(i = 1,\cdots,n)$ 和目标点云 $\boldsymbol p_i^t(i = 1,\cdots,m)$ ，点云粗配准完成的工作主要就是求解两团刚体之间的 $\boldsymbol R$ 和 $\boldsymbol t$ ，使得原始点云 $\boldsymbol p_i^s$ 通过变换得到新的点云 $\boldsymbol p_i^{s_{new}} = \boldsymbol R \boldsymbol p_i^s + \boldsymbol t$ 和目标点云 $\boldsymbol p_i^t$ 之间最接近，即两者之间最近邻距离小于 $\varepsilon$ 的点云数目最大化，该准则被称为LCP（Largest Common Points）

假设现在找到了 $k$ 个点云对 $\boldsymbol p_i^t$ 和 $\boldsymbol p_i^s,i = 1,\cdots,n$ ，那么点云配准就是在求解最优的 $\boldsymbol R^*,\boldsymbol t^*$ 使得满足如下的式子
$\boldsymbol R^*,\boldsymbol t^* = \arg\min_{\boldsymbol R,\boldsymbol t}\frac{1}{k} \sum_{i =1}^{k}w_i||\boldsymbol R \boldsymbol p_i^s + \boldsymbol t - \boldsymbol p_i^t||^2$

求解上述问题的常用步骤如下

从目标点云和源点云中找到点对 $\boldsymbol p_i^t,\boldsymbol p_i^s$
根据权重，计算等效中心 $\overline{\boldsymbol p^t} = \frac{\sum_{i = 1}^k w_i \boldsymbol p_i^t}{\sum_{i = 1}^k w_i}, \overline{\boldsymbol p^s} = \frac{\sum_{i = 1}^k w_i \boldsymbol p_i^s}{\sum_{i = 1}^kw_i}$
将每个点去中心化 $\hat{\boldsymbol p_i^s} = \boldsymbol p_i^s - \overline{\boldsymbol p^s},\hat{\boldsymbol p_i^t} = \boldsymbol p_i^t - \overline{\boldsymbol p^t}$
对协方差矩阵进行SVD分解 $\Sigma = \boldsymbol U \boldsymbol \Lambda \boldsymbol V^T$ ，其中协方差矩阵 $\Sigma$ 为
$\Sigma = \begin{bmatrix} \hat{\boldsymbol p_1^s} & \hat{\boldsymbol p_2^s} & \cdots & \hat{\boldsymbol p_k^s} \end{bmatrix} \begin{bmatrix} w_1 & 0 & \cdots & 0 \\ 0 & w_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & w_k \end{bmatrix} \begin{bmatrix} \hat{\boldsymbol p_1^t}^T \\ \vdots \\ \hat{\boldsymbol p_k^t}^T \end{bmatrix}$
求解得到旋转矩阵 $\boldsymbol R^* = \boldsymbol V \begin{bmatrix} 1 & 0 & \cdots & 0 \\ 0 & 1 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & |\boldsymbol V \boldsymbol U^T| \end{bmatrix}\boldsymbol U^T$
求解得到平移向量 $\boldsymbol t^* = \overline{\boldsymbol p^t} - \boldsymbol R^* \overline{\boldsymbol p^s}$

注意到在每个点集中选取3组点就可以进行上述的计算，然后根据LCP准则得到最优的 $\boldsymbol R^*$ 和 $\boldsymbol t^*$ ，因此使用穷举法的复杂度是 $O(n^3m^3k)$ ，如果点云数量在 $10^4$ 量级，该复杂度在工程上是不可接受的。为了降低复杂度，后面涌现了一大批算法，其本质思想就是在选择点对的时候根据某些条件去掉明显不可理的点对，这些算法的区别就在于如何设计这些条件。

因此有人提出了RANSAC算法，从源点云中随机提取3个点，然后穷举目标点云中所有3点对计算旋转矩阵，然后根据旋转矩阵计算距离小于 $\varepsilon$ ，重复此步骤 $L$ 次，根据LCP准则从这 $L$ 次中选取最优的 $\boldsymbol R^*$ 和 $\boldsymbol t^*$ ，该方法的复杂度是 $O(Lm^3k)$ ，通过调节 $L$ 在精度和速度之间取得平衡。

4PCS算法在源点云中选取四个点，如果是刚体变换的话，目标点云中对应的四个点同样需要满足一定的条件，这四个点需要满足一定的条件，通过该条件来筛选原始点对，减少计算量。

点云粗配准

推荐阅读更多精彩内容