ICP算法

ICP（Iterative Closest Point），即迭代最近点算法，是经典的数据配准算法。其特征在于，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阀值，则迭代进行上述运算直到满足给定的误差要求.

ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。

Align 3D Data

如果空间中两组点云之间的对应关系已经明确，则很容易求得两者之间的刚性变换，即旋转和平移共6个参数，但这种对应关系一般很难事先知道。
ICP算法假设两组点云之间的对应关系由最近点确定，一步步将源点云$P$匹配到目标点云$Q$。

ICP算法主要包含对应点确定和变换计算更新，简要流程如下

1. 在源点云 $P$ 中选择一些随机点 $p_i, i=1,2, \cdots,n$
2. 在目标点云 $Q$ 中找到每个点 $p_i$ 的最近点 $q_i$
3. 剔除一些距离较远的点对
4. 构建距离误差函数$E$
5. 极小化误差函数，如果对应点距离小于给定阈值设置，则算法结束；否则根据计算的旋转平移更新源点云，继续上述步骤。

Basic ICP

传统的ICP算法主要有两种度量函数，即point-to-point和point-to-plane距离，一般来说，point-to-plane距离能够加快收敛速度，也更加常用。

• Point-to-Point Error Metric

$$E_{p o i n t}=\sum_{i}\left\|\mathrm{R} p_{i}+t-q_{i}\right\|^{2}$$

• Point-to-Plane Error Metric

$$E_{plane} = \sum_{i}\left[\left(\mathrm{R} p_{i}+t-q_{i}\right) \cdot n_{q_i}\right]^{2}$$

Colored ICP

Colored ICP算法[Park2017]针对有颜色的点云，在原始point-to-plane能量项的基础上，增加了一个对应点对之间的颜色约束，能够有更好的配准结果。其目标函数如下

$$E_{color} = (1-\delta) E_{C}+\delta E_{G}$$

其中$E_{C}$是颜色能量项，$E_{G}$是几何能量项， $$\delta \in[0,1]$$ 是两者之间的相对权重。几何能量项就是传统的point-to-plane能量项：

$$E_{G}=\sum_{i}\left[(R p_i + t - q_i) \cdot n_{q_i}\right]^{2}$$

而颜色项衡量点$p_i$的颜色$\boldsymbol{C}(p_i)$与其投影到$q_i$的切平面上的颜色之差：

$$E_{C}=\sum_{i}\left(\boldsymbol{C}_{q_i}(\Pi(R p_i + t))-\boldsymbol{C}(p_i)\right)^{2}$$

其中$\Pi$将3D点投影到切平面，$\boldsymbol{C}_{q_i}$是提前计算好的定义在$q_{i}$的切平面上的连续函数。

Symmetrized ICP

Symmetrized ICP算法极小化到点到由$n_p$和$n_q$决定的平面的距离平方和，同时优化拆分的旋转 :

$$E_{two-plane}=\sum_{i}\left[\left(\mathrm{R} p_{i}-\mathrm{R}^{-1} q_{i}+t\right) \cdot\left(\mathrm{R} n_{p_i}\right)\right]^{2} + \left[\left(\mathrm{R} p_{i}-\mathrm{R}^{-1} q_{i}+t\right) \cdot\left(\mathrm{R}^{-1} n_{q_i}\right)\right]^{2}$$

Symmetric ICP

Symmetric ICP[Rusinkiewicz2019]是ICP算法的另一种改进。其核心是在原有点到面距离$(p-q) \cdot n_{q}$上做了一个微小的改动即$(p-q) \cdot\left(n_{p}+n_{q}\right)$，在几乎不增加计算量的基础上，能够有更快更可靠的收敛。其目标函数有两种，第一种是法向旋转版本(Rotated-Normals)的对称ICP目标函数：

$$E_{symm-RN}=\sum_{i}\left[\left(\mathrm{R} p_{i}-\mathrm{R}^{-1} q_{i}+t\right) \cdot\left(\mathrm{R} n_{p_i}+\mathrm{R}^{-1} n_{q_i}\right)\right]^{2}$$

另一种更简单但效果差不多的版本是

$$E_{symm}=\sum_{i}\left[\left(\mathrm{R} p_{i}-\mathrm{R}^{-1} q_{i}+t\right) \cdot\left(n_{p_i}+n_{q_i}\right)\right]^{2}$$

将旋转拆分为两个相同的部分在进行目标函数线性化估计时会带来一些好处，因为在线性化时假设旋转角度很小，优化拆分的旋转即角度的一半使得近似误差更小。

Solve

ICP算法在极小化能量时通常都需要求解一个非线性最小二乘问题，但可以线性化，假设$\theta \approx 0$，则$\sin(\theta) \approx \theta$，$\cos(\theta) \approx 1$，忽略二次项，可以得到一个线性的最小二乘问题，再用Gauss-Newton或者Levenberg-Marquardt算法求解。

Algorithm

Go-ICP

Go-ICP即Globally optimal ICP，提出了在L2误差度量下欧式空间中匹配两组点云的全局最优算法。

Sparse ICP

Code

• ICP: libicp Iterative-Closest-Point Go-ICP
• Sparse ICP: sparseicp icpSparse
• CUDA
  • ICPCUDA
  • CudaICP
  • ICP
  • pose_refine
• OpenCL
  • ICP

Reference

• Dynamic Geometry Processing

• [Rusinkiewicz2019] Szymon Rusinkiewicz. A Symmetric Objective Function for ICP, SIGGRAOH 2019

• [Park2017] J. Park, Q.-Y. Zhou, and V. Koltun. Colored Point Cloud Registration Revisited, ICCV 2017.

• [Bouaziz2013] Sofien Bouaziz, Andrea Tagliasacchi, Mark Pauly. Sparse Iterative Closest Point, SGP 2013.