3D-3D correspondence

2組の対応する3D点集合 $\{\mathbf{p}_i\}$ (ソース)と $\{\mathbf{q}_i\}$ (ターゲット)が与えられているとき、3D-3Dレジストレーション問題はそれらを整合させる剛体変換を求める:

$\min_{R,\mathbf{t}} \sum_i \|\mathbf{q}_i - (R\mathbf{p}_i + \mathbf{t})\|^2$

これは両フレームが3Dデータを提供する場合 ―― RGB-Dやステレオフレーム、LiDARスキャン、あるいは統合される2つの点群マップ ―― に発生する。古典的なアルゴリズムは**ICP(Iterative Closest Point)**である。

閉形式SVD解(対応関係が既知の場合)

対応関係が既知であれば、最適な位置合わせは閉形式で得られる:

重心を計算する: $\bar{\mathbf{p}} = \frac{1}{n}\sum_i \mathbf{p}_i$ 、 $\bar{\mathbf{q}} = \frac{1}{n}\sum_i \mathbf{q}_i$ 。
相互共分散を計算する: $H = \sum_i (\mathbf{p}_i - \bar{\mathbf{p}})(\mathbf{q}_i - \bar{\mathbf{q}})^T$ 。
SVD: $H = U\Sigma V^T$ 。
回転: $R = VU^T$ ( $\det(R) = -1$ の場合は符号補正を行う)。
並進: $\mathbf{t} = \bar{\mathbf{q}} - R\bar{\mathbf{p}}$ 。

なぜ最初に重心を求めるのか? コストを $\mathbf{t}$ について微分してゼロと置くと、最適な並進は常にソースの重心をターゲットの重心へ写すことが分かる。これを代入し直すと問題は分離される: 回転は中心化された点だけで単独に求められ、その後ステップ5で並進が続く。同じ構成に最適スケール係数 $s$ を加えて拡張したもの(Umeyama位置合わせ)は、軌道評価ツールがATEを計算する前に推定軌道とグラウンドトゥルースを位置合わせするために用いるものである ―― スケールが観測不能な単眼SLAMにおいては、スケール整合版が必須となる。

ICPの反復部分

対応関係が未知の場合、ICPは以下を交互に繰り返す:

対応関係ステップ: 各ソース点について、ターゲット内の最近傍を見つける(kd-treeにより高速化)。
位置合わせステップ: 上記の閉形式問題を解き、変換を適用する。

収束するまで繰り返す。ICPは合理的な初期推定を必要とし(局所最小解に収束する)、外れ値に敏感であるため、実用的な変種は距離の閾値、ロバストカーネル、法線に基づく除外を追加する。

Point-to-planeとその先

Point-to-plane ICPは、点対点の指標を、ターゲットの法線 $\mathbf{n}_i$ を用いた点対曲面の指標に置き換える:

$\min_{R,\mathbf{t}} \sum_i \big(\mathbf{n}_i^T\,(R\mathbf{p}_i + \mathbf{t} - \mathbf{q}_i)\big)^2$

点は表面に沿って滑ることが許され、表面に垂直な方向にのみペナルティが課される ―― 「最近傍点」が真の対応点であることはまれであるという現実に合致している。これは滑らかな表面上ではるかに速く収束し、RGB-DやLiDARパイプラインで主流となっている(KinectFusionが用いているのもこれである)。このコストはもはや閉形式ではなく、回転を線形化(小角近似)して反復ごとに $6\times6$ の線形系を解くことになる。同じ系列の更なる改良には、Generalized-ICP(平面対平面、共分散重み付け)や、LOAMのようなLiDARオドメトリで用いられるpoint-to-lineの指標がある。

よくある落とし穴

不良な初期化: ICPの収束域は狭い。オドメトリ、等速運動モデル、あるいは大域的手法から種を与えること ―― 大きな運動の後に単位行列から始めてはならない。
部分的な重なり: 真の対応を持たない点は推定を引っ張る。距離パーセンタイルやロバストカーネルによって対応関係をトリミングすること。
退化した幾何: 単一の平面は3自由度を制約しないまま残す(2並進+平面内1回転)。長い通路やトンネルも同様に解を制約不足にする ―― $6\times6$ の正規方程式行列の固有値によって退化を検出すること。
SVDによる反射: 常に $\det(R) = -1$ の符号補正を適用すること。これを忘れると、ノイズのあるデータや平面データ上で鏡像化された「位置合わせ」が得られる。

SLAMにおける意義

ICPは密なRGB-D SLAM(KinectFusionはpoint-to-plane ICPで各深度フレームをモデルに位置合わせする)や、ほとんどのLiDARオドメトリシステムのトラッキングエンジンであり、SVDによる位置合わせステップは2つのサブマップを統合する必要があるたびにループクロージングで再登場する。2D-2D(深度なし)と2D-3D(片側のみ深度あり)とともに、対応関係のツールボックスを完成させる: どれを使うかは単に3D情報がどこで得られるかによって決まる。

ハンズオン

ICPハンズオン