Iterative Closest Point

• 又稱: ICP, ICP registration
• 類別: State estimation & navigation › Localisation
• 標籤: point-cloud, registration, lidar, localization, 3d-perception

一種對齊兩個點雲的演算法，反覆把每個點與另一個點雲中的最近鄰配對，並求出最能擬合這些點對的剛體轉換。

定義

迭代最近點（Iterative Closest Point，ICP）是一種對位兩組 3D 點的演算法，也就是找出讓其中一組與另一組最佳對齊的旋轉與平移。它由 Besl 與 McKay 於 1992 年提出，Chen 與 Medioni 也在差不多同一時間發表了一種密切相關的方法。ICP 是光達（LiDAR）里程計、掃描匹配、3D 重建與物體姿態精修的基本工具。

這個演算法交替執行兩個步驟。首先，對來源點雲中的每個點，在目標點雲中找出最近的點，得到一組暫定的對應關係。其次，計算讓這些點對之間距離最小的剛體轉換；對點到點的距離而言，這有使用奇異值分解的封閉解。來源點雲依該轉換移動，接著重複這個過程，直到兩次迭代之間的變化變得很小。

ICP 只會收斂到局部極小值，因此需要合理的初始猜測，通常來自里程計、IMU 或某種粗略的全域對位方法。如果起始的錯位很大，最近鄰就會是錯的，演算法可能鎖定在不正確的對齊上。

許多變體改善了速度與穩健性。點到平面 ICP 量測每個點到其匹配點切平面的距離，在結構化場景中通常收斂得更快。Generalized ICP 會在兩個點雲上都對局部表面共變異數建模。實際的實作還會對點進行降取樣、以 k-d 樹搜尋鄰近點，並剔除距離過遠或表面法線不一致的點對。

為何重要

大多數以光達（LiDAR）為基礎的定位與建圖系統，都使用 ICP 或其變體來估計機器人在兩次掃描之間如何移動，並驗證迴圈閉合。在有物體 3D 模型的情況下，它也是抓取前精修物體姿態的標準方法。

主要應用

• 移動機器人的光達（LiDAR）里程計與掃描匹配
• 在 SLAM 中驗證迴圈閉合
• 從深度資料精修物體姿態以進行抓取
• 把多次掃描合併成單一 3D 模型
• 將即時感測器資料與事先建立的地圖對齊以進行定位

實際應用

為移動機器人建置光達（LiDAR）里程計的團隊，通常會以體素網格對每次掃描降取樣，用上一次的運動估計或積分後的 IMU 資料作為初始猜測，並對上一次掃描或局部地圖執行點到平面 ICP。PCL、Open3D 與 libpointmatcher 等函式庫都提供現成的實作。主要的調整參數是最大對應距離、收斂門檻與迭代次數上限；工程師會檢查最終的適配度分數，以抓出在長走廊或空曠場地等缺乏幾何特徵之處的失敗，因為 ICP 在那些地方可能沿著不受限制的方向滑移。

常見誤解

• ICP 能找到全域最佳的對齊 — 它會收斂到局部極小值，需要良好的初始猜測
• ICP 適用於任何幾何形狀 — 在平坦地板或長隧道等退化場景中，有些方向不受限制，結果可能漂移
• ICP 能處理縮放與變形 — 標準 ICP 只估計剛體轉換；有縮放或非剛性的對齊需要其他方法

誰需要了解

• 使用光達（LiDAR）與深度攝影機的感知工程師
• SLAM 與定位開發者
• 進行以模型為基礎之物體姿態估計的操作工程師
• 3D 掃描與重建的實務工作者

相關技術

• 點雲函式庫（Point Cloud Library，PCL）
• Open3D
• 常態分布轉換（Normal Distributions Transform，NDT）
• Generalized ICP
• k-d 樹（k-d Tree）
• RANSAC

相關術語

• Point Cloud
• LIDAR
• SLAM
• Transform

相關專案

• Open3D: Modern library for 3D point cloud processing, mesh operations, and 3D machine learning
• oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
• PCL: Industry-standard C++ library for point cloud processing in robotics perception
• Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
• OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
• Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots

影片

• Iterative Closest Point (ICP) - Computerphile: Computerphile
• Iterative Closest Point (ICP) - 5 Minutes with Cyrill: Cyrill Stachniss