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.