ICP演算法實現（MATLAB）

ICP原理

ICP(Iterative closet point method)迭代最近點法，用於兩組資料之間的配准，其實現的具體步驟如下

對於兩組點云： P P、 Q Q

step1：選擇控制點 pi→∈P \vec{p_{i}}\in{P}、設定 T T的初始值 T0=T0 T^{0}=T_{0}

step2：重複執行以下步驟，直至滿足收斂條件

step2-1：對各控制點， pi→ \vec{p_{i}}在 Q Q中求其最近點 qj→ \vec{q_{j}}，並將其作為 pi→ \vec{p_{i}}的假想對應點

step2-2：對於確定的對應的關係，求解 Tk T^k，對並且求解loss function
Ek=∑Tk|pi→−qj→|2> E^k=\sum{T^k|\vec{p_{i}}-\vec{q_{j}}|^{2}}>

step2-3：重新計算控制點 pi→ \vec{p_{i}}在經過 Tk T_{k}變換之後的點，並將其重新賦值給 pi→ \vec{p_{i}}

演算法的收斂條件是 δ=<