Iterative Closest Point

• 別名: ICP, ICP registration
• カテゴリ: State estimation & navigation › Localisation
• タグ: point-cloud, registration, lidar, localization, 3d-perception

各点をもう一方の点群の最近傍と対応付け、それらの組に最もよく合う剛体変換を求めることを繰り返して、二つの点群を位置合わせするアルゴリズム。

定義

反復最近点（Iterative Closest Point、ICP）は、二つの 3D 点の集合を位置合わせする、すなわち一方を他方に最もよく重ね合わせる回転と並進を求めるアルゴリズムである。1992 年に Besl と McKay によって提案され、ほぼ同じ時期に 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