點雲配准算法ICP

ICP算法
迭代最近點(Iterative Closest Point), 常用於做點雲匹配(對齊)和激光雷達標定場景中。
本質上是最小二乘法的一個典型應用。
核心思想
把當前幀和上一幀距離最近的兩點看成爲同一點，即形成一組點對，
再計使用最小二乘法算出最優的那組轉換參數，確定旋轉矩陣和平移向量，從而確定轉換關係，

使用下面的數學關係來描述一下，
設當前幀點雲中的點爲$p_i, p_i\in C$
設上一幀點雲中的點爲$p_i^{'}, p_i^{'}\in D$
那麼應滿足以下的轉換模型(這個模型是已知公式)
$p_i^{'}=R\cdot p_i + t$
其中，$R$代表旋轉矩陣，$t$代表平移向量，再進一步抽象可將上式表示爲
$D=R\cdot C +t$

ICP算法的主要步驟

1. 任取$p_i\in C$, 並遍歷$D$，尋找與$p_i$距離最小的點$p_i^{'}$, 形成$m$對點

2. 將這$m$對點$(p_i,p_i^{'})$帶入以下公式，使用最小二乘法計算
   $\argmin_{R,t}E = \frac{1}{2} \sum_{i=1}^m\parallel p_i^{'} - (R\cdot p_i+t) \parallel_2^2$

3. 如果迭代的誤差小於指定的閾值，即 $\Delta E<e$, 或者達到了最大迭代次數，則把當前計算得到的$R,t$作爲結果返回，否則執行步驟 (4)

4. 使用當前求得的$R,t$更新點集，$p_i^{'} =R\cdot p_i+t$ ; $p_i=p_i^{'}$; 轉到步驟(1)