ICP（Iterative Closest Point，迭代最近點）

定義

ICP（Iterative Closest Point，迭代最近點）是一種點雲配準（registration）演算法：給定兩組點雲，找出一個旋轉 RR 與平移 t\mathbf{t}，讓其中一組點雲經過轉換後，與另一組最貼合。

演算法流程

  1. 配對：為來源點雲的每個點，在目標點雲中找最近的點。
  2. 求轉換：計算讓所有配對距離平方和最小的 R,tR, \mathbf{t}：
min⁡R,t∑i∥R pi+t−qi∥2\min_{R,\mathbf{t}} \sum_{i} \left\| R\,\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i \right\|^2

這一步對 point-to-point 有封閉解（以 SVD 求解）。 3. 套用轉換，回到第 1 步，直到誤差不再明顯下降。

使用情境

  • 光達里程計：相鄰兩幀掃描做 ICP，推算機器人移動了多少。
  • 地圖拼接與迴圈閉合：確認回到同一地點時，兩次掃描的相對位姿。
  • 物體位姿估計：把 CAD 模型點雲對齊到相機量到的點雲，用於抓取。

常見誤解

  • ICP 能處理任何初始偏差？ 不能，它只保證收斂到局部最佳解。
  • 點越多越好？ 點數太多會拖慢最近點搜尋，實務上會先做降採樣（voxel grid）與離群點移除。

延伸閱讀

常見問題

faq_01.log
ICP 為什麼需要好的初始值？
ICP 每一輪都用「目前位置下的最近點」當配對，如果初始位置差太多，配對大多是錯的，會收斂到錯誤的局部最佳解。實務上常用輪速計或 IMU 給初始猜測，或先做全域粗配準（如 FPFH 特徵配對）再用 ICP 精修。
faq_02.log
point-to-point 和 point-to-plane ICP 差在哪？
point-to-point 最小化對應點之間的距離；point-to-plane 最小化點到對應平面的距離，對牆面、地面這類平坦結構收斂更快更準，是光達 SLAM 常用的變體。

相關術語

光達（LiDAR）SLAM（同時定位與建圖）Point Cloud（點雲）