点云配准与ICP

点云配准与ICP

关于点云

  • 点云是在同一空间参考系下表达目标空间分布和目标表面特性的海量点集合，在获取物体表面每个采样点的空间坐标后，得到的是点的集合，称之为点云（Point Cloud）
  • 点云一般包括根据激光测量得到的点云和根绝摄影测量得到的点云
    • 激光测量的点云包括三维坐标（XYZ）和激光反射强度（Intensity）。强度信息与目标的表面材质、粗糙度、入射角方向，以及仪器的发射能量，激光波长有关
    • 摄影测量原理得到的点云，包括三维坐标（XYZ）和颜色信息（RGB）
    • 也有把激光和摄影相结合在一起的（多传感器融合技术），这种结合激光测量和摄影测量原理得到点云，包括三维坐标（XYZ）、激光反射强度（Intensity）和颜色信息（RGB）

什么是点云配准

点云配准（Point Cloud Registration）指的是输入两幅点云$P_s$（Source）和$P_t$（Target） ，输出一个变换$T$使得$T(P_s)$和$P_t$的重合程度尽可能高。变换$T$可以是刚性的（Rigid），也可以不是，下面只考虑刚性变换，即变换只包括旋转（$R$）、平移（$t$）。

点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。

目前应用最广泛的点云精配准算法是迭代最近点算法（Iterative Closest Point，ICP）及各种变种ICP算法。

ICP，局部点云配准

ICP，Iterative Closest Point，即迭代最近点算法，是应用最广泛的3D点云配准算法之一， 其通过欧式变换求解出两片点云的旋转平移矩阵及对应的配准误差。

算法描述

对于$T$是刚性变换的情形，点云配准问题可以描述为

$$
R^*,t^*=\mathop{arg\ min}\limits_{R,t}\frac{1}{|P_s|}\sum_{i=1}^{|P_s|}||p_t^i-(R\cdotp_s^i+t)||^2
$$

其中$p_s,p_t$是源点云和目标点云中的对应点

算法流程

  1. 点云预处理，如滤波、清洗等
  2. 利用一些点解出的变换，寻找最近点
  3. 调整部分对应点的权重
  4. 删除不合理对应点
  5. 计算Loss，最小化Loss并求解当前最优变换
  6. 重复2-6，迭代收敛

寻找最近点

利用初始$R_0,t_0$或上一次迭代得到的$R_{k-1},t_{k-1}$对初始点云进行变换，得到一个临时的变换点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点。

如果直接进行比较找最近邻点，需要进行两重循环，计算复杂度为 $O(|P_s|\cdot |P_t|)$，这一步会比较耗时，常见的加速方法有

  • 设置距离阈值，当点与点距离小于一定阈值就认为找到了对应点，不用遍历完整个点集
  • 使用 ANN(Approximate Nearest Neighbor) 加速查找，常用的有 KD-tree；KD-tree 建树的计算复杂度为$O(N\log(N))$，查找通常复杂度为$O(\log(N))$，最坏情况下$O(N)$

求解最优变换

对于Point-to-Point ICP问题，求最优变换是有闭形式解（Closed-Form Solution）的，可以借助SVD分解来计算。

  • 结论

    在已知点的对应关系的情况下，设$\overline{p}_s,\overline{p}_t$分别表示源点云和目标点云的质心，令$\hat{p}_s^i=p_s^i-\overline{p}_s^i\ ,\ \ \hat{p}_t^i=p_t^i-\overline{p}t^i\ ,\ \ H=\sum{i=1}^{|P_s|}\hat{p}_s^i{\hat{p}_t^i}^T$，这是一个$3\times 3$矩阵，对$H$进行SVD分解得到$H=U\Sigma V^T$，则Point-to-Point ICP的最优解为

    $$
    R^*=VU^T
    $$

    最优平移为

    $$
    t^*=\overline{p}_t-R^*\cdot\overline{p}_s
    $$

  • 最优平移

    令$N=|P_s|$，设$F(t)=\sum_{i=1}^N||(R\cdot p_s^i+t)-p_t^i||^2$，对其进行求导，则有

    $$
    \begin{aligned}\frac{\partial F}{\partial t}&=\sum_{i=1}^N2(R\cdotp_s^i+t-p_t^i)\&=2nt+2R\sum_{i=1}^Np_s^i-2\sum_{i=1}^Np_t^i\end{aligned}
    $$

    令导数为0，得

    $$
    \begin{aligned}t&=\frac{1}{N}\sum_{i=1}^Np_t^i-R\frac{1}{N}\sum_{i=1}^Np_s^i\&=\overline{p}_t^i-R\cdot\overline{p}_s^i\end{aligned}
    $$

    无论$R$取值如何，根据上式我们都可以求得最优的 t，使得 loss 最小

  • 最优旋转

    经过最优平移的推导，我们知道无论旋转如何取值，都可以通过计算点云的质心来得到最优平移，为了下面计算上的简便，我们不妨不考虑平移的影响，先将源点云和目标点云都转换到质心坐标下，这也就是之前令$\hat{p}_s^i=p_s^i-\overline{p}_s^i\ ,\ \ \hat{p}_t^i=p_t^i-\overline{p}_t^i$的意义。

    不考虑平移，则Loss可以写成

    $$
    F(R)=\sum_{i=1}^N||R\cdot \hat{p}_s^i-\hat{p}_t^i||^2
    $$

    化简有

    $$
    \begin{aligned}||R\cdot \hat{p}_s^i-\hat{p}_t^i||^2&=(R\cdot \hat{p}_s^i-\hat{p}_t^i)^T(R\cdot \hat{p}_s^i-\hat{p}_t^i)\&=({\hat{p}_s^i}^TR^T-{\hat{p}_t^i}^T)(R\cdot \hat{p}_s^i-\hat{p}_t^i)\&={\hat{p}_s^i}^TR^TR\hat{p}_s^i-{\hat{p}_t^i}^TR{\hat{p}_s^i}-{\hat{p}_s^i}^TR^T{\hat{p}_t^i}+{\hat{p}_t^i}^T{\hat{p}_t^i}\&=||{\hat{p}_s^i}||^2+||{\hat{p}_t^i}||^2-{\hat{p}_t^i}^TR{\hat{p}_s^i}-{\hat{p}_s^i}^TR^T{\hat{p}_t^i}\&=||{\hat{p}_s^i}||^2+||{\hat{p}_t^i}||^2-2{\hat{p}_t^i}^TR{\hat{p}_s^i}\end{aligned}
    $$

    其中，$R^TR=I,{\hat{p}_t^i}^TR{\hat{p}_s^i}={\hat{p}_s^i}^TR^T{\hat{p}_t^i}$

    由于点的坐标是确定的（和$R$无关），所以最小化原Loss等价于求

    $$
    R^*=\mathop{arg \ min}\limits_{R}(-2\sum_{i=1}^N{\hat{p}_t^i}^TR{\hat{p}_s^i})
    $$

    由于$trace(AB)=trace(BA)$，即有

    $$
    \begin{aligned}R^*&=\mathop{arg \ max}\limits_{R}(\sum_{i=1}^N{\hat{p}t^i}^TR{\hat{p}s^i})\&=\mathop{arg \ max}\limits{R} \ trace(P_t^TRP_s)\&=\mathop{arg \ max}\limits{R} \ trace(RP_sP_t^T)\end{aligned}
    $$

    带入SVD分解，得

    $$
    \begin{aligned}trace(RP_sP_t^T)&=trace(RH)\&=trace(RU\Sigma V^T)\&=trace(\Sigma V^TRU)\end{aligned}
    $$

    由于$V,U,R$都是正交矩阵，所以$V^TRU$也是正交矩阵，令

    $$
    M=V^TRU=\left[\begin{aligned}m_{11}& &m_{12}& &m_{13}\m_{21}& &m_{22}& &m_{23}\m_{31}& &m_{32}& &m_{33}\end{aligned}\right]
    $$

    则有

    $$
    \begin{aligned}trace(\Sigma V^TRU)&=trace(\Sigma M)\&=\sigma_1m_{11}+\sigma_2m_{22}+\sigma_1m_{33}\end{aligned}
    $$

    根据奇异值非负的性质和正交矩阵的性质（正交矩阵中的元素绝对值不大于 1），容易证得只有当$M$为单位阵时$trace(\Sigma M)$最大，即

    $$
    V^TRU=I\R=VU^T
    $$

    所以有$R^*=VU^T$

    最后还需要进行 Orientation rectification，即验证$R^*=VU^T$是不是一个旋转矩阵（检查是否有$|R|=1$），因为存在$|R|=-1$的可能，此时$R$表示的不是旋转而是一个Reflection，所以我们还要给上述优化求解加上一个$|R|=1$的约束，详见Reference

    计算步骤为

    1. 计算源点云和目标点云质心
    2. 将源点云和目标点云转换到质心坐标系
    3. 计算矩阵$H$
    4. 对$H$求 SVD 分解，根据公式求得$R^*$
    5. 根据公式计算$t^*$

  • 迭代

    每一次迭代我们都会得到当前的最优变换参数$R_k,t_k$，然后将该变换作用于当前源点云。找最近对应点和求解最优变换这两步不停迭代进行，直到满足迭代终止条件，常用的终止条件有

    • $R_k,t_k$的变化量小于一定值
    • Loss变化量小于一定值
    • 达到最大迭代次数

优缺点

优点

  • 简单，不必对点云进行分割和特征提取
  • 初值较好情况下，精度和收敛性都不错

缺点

  • 找最近对应点的计算开销较大
  • 只考虑了点与点距离，缺少对点云结构信息的利用

Reference

点云配准综述

三维点云配准 – ICP 算法

3D Basic
#point cloud
点云配准与ICP
https://alschain.com/2022/06/25/点云配准与ICP/
作者
Alschain
发布于
2022年6月25日
许可协议