39 ICP 位置合わせと3D校正
同一のワークを2つの視点からそれぞれ1回ずつスキャンすると2枚の点群が得られる；これらを完全な表面につなぎ合わせるには、まず両者を位置合わせしなければならない。スキャンした実物を設計されたCADモデルと比較し、アセンブリが適切に行われているかを判断するには、まず実物の点群をモデルの座標系に「配置」しなければならない。ロボットがビンから部品を把持するには、まずその部品のハンドに対する正確な姿勢を知らなければならない——一見無関係に見えるこの3つの事柄には、共通の骨格がある：一方の点群を剛体変換によって他方の点群上に移動させ、両者を可能な限り一致させることである。これが3次元の位置合わせ（レジストレーション）であり、ICP（iterative closest point：反復最近傍点）は工業現場で位置合わせを担う主力アルゴリズムである。
本章では1つの合成シーンで全実験を行う（図 39.1）：1枚はモデル点群——コンベヤ平面上に立方体と球冠が置かれたもの、合計5678点；もう1枚はスキャン点群——モデルに既知の剛体変換（軸\((1,2,2)\)回りに\(12°\)回転、並進\((5,-3,2)\,\text{mm}\)）を施し、\(\sigma=0.1\,\text{mm}\)の測定ノイズを重畳し、片側を裁断し、右側にモデルには存在しない「侵入壁」を追加したもの、合計6497点である。両点群のうち真に対応する点はわずか4310点であり、残りの2187点（33.7%）は対応のない非重複部分である。図中ではモデルが青、スキャンが赤で示され、初期状態では明らかに位置がずれている；右側の孤立した赤い縦壁が侵入構造である——これはモデルに対応点を持たないが、アルゴリズムはこれを識別して除外しなければならない。この「完全には重複しない」設定は自分自身を困らせるためのものではない：実際のスキャンでは、カメラの視野境界、隣接部品、ビンの壁による非重複はほぼ常態である。
39.1 位置合わせ問題とICP
剛体位置合わせで求解すべきものは、回転行列\(\mathbf R\)（\(3\times3\)、直交かつ\(\det\mathbf R=1\)）と並進ベクトル\(\mathbf t\)であり、ソース点群の各点\(\mathbf p_i\)を変換した結果がターゲット点群と一致する：\(\mathbf R\mathbf p_i+\mathbf t \approx \mathbf q_i\)。これは@sec-roi_and_fixturing で2次元の位置補正により平面姿勢\((x,y,\theta)\)を求める問題を3次元に拡張したものである——2次元には3つの自由度があるのに対し、3次元の剛体変換には6つの自由度（3つの回転 + 3つの並進）がある。
難点は、ソース点群のどの点がターゲット点群のどの点に対応するかを事前には知らないことである。対応関係と変換は互いに因果関係にある——対応関係がわかれば変換を解くことができ、変換がわかれば対応関係を探すことができる。ICPは素朴だが効果的な反復によってこの鶏が先か卵が先かのデッドロックを打破し、各ラウンドで3つの事柄を実行する：
- 最近傍による対応探索：ソース点群を現在の推定値\((\mathbf R,\mathbf t)\)で変換した後、各ソース点についてターゲット点群内で最近傍対応を探索する。このステップはICPの計算ボトルネックであり、kd木を用いて総当たりの\(O(NM)\)の最近傍探索を\(O(N\log M)\)に低減しなければならない（チャプター 37）。
- 遠い対応の棄却：最近傍が「真の」対応であるとは限らない——侵入壁上の点も最も近いモデル点が割り当てられるが、この対応は偽である。対応距離によって外れ値ペアを棄却することは、ICPが非重複部分を処理するための重要なゲートである。本章では中央値法を用いる：全対応距離の中央値\(\tilde d\)を求め、距離が\(\le 2.5\,\tilde d\)の対応を保持し、侵入壁のような大きな距離の対応を排除する。
- 最適剛体変換の求解：フィルタリングされた対応について、対応点ペア間の距離の二乗和を最小にする剛体変換を求める。このステップには閉形式解が存在し、それがKabsch / Umeyamaアルゴリズムである。
第3ステップの数学的内容は詳しく説明する価値があり、これは@sec-math_preliminaries のSVDの優れた応用例である。フィルタリング後に\(n\)組の対応\(\{(\mathbf p_i,\mathbf q_i)\}\)があるとし、次の量を最小化する：
\[ E(\mathbf R,\mathbf t)=\sum_{i=1}^{n}\bigl\|\mathbf R\mathbf p_i+\mathbf t-\mathbf q_i\bigr\|^2 . \]
まず2つの点群からそれぞれ重心を減算する：\(\bar{\mathbf p}=\frac1n\sum_i\mathbf p_i\)、\(\bar{\mathbf q}=\frac1n\sum_i\mathbf q_i\)とし、\(\mathbf p_i'=\mathbf p_i-\bar{\mathbf p}\)、\(\mathbf q_i'=\mathbf q_i-\bar{\mathbf q}\)と定義する。最適な並進は2つの重心を一致させ、回転は重心減算後の点のみに依存することが証明できる。\(3\times3\)の相互共分散行列を構成し：
\[ \mathbf H=\sum_{i=1}^{n}\mathbf p_i'\,\mathbf q_i'^{\,\top}, \]
これに特異値分解\(\mathbf H=\mathbf U\boldsymbol\Sigma\mathbf V^\top\)を施すと、最適な回転と並進は
\[ \mathbf R=\mathbf V\,\mathrm{diag}(1,1,d)\,\mathbf U^\top,\quad d=\operatorname{sign}\!\bigl(\det(\mathbf V\mathbf U^\top)\bigr),\qquad \mathbf t=\bar{\mathbf q}-\mathbf R\,\bar{\mathbf p}. \]
となる。中央の\(\mathrm{diag}(1,1,d)\)が重要な点である：単純な\(\mathbf V\mathbf U^\top\)では行列式が\(-1\)の「鏡像」（回転ではなく反射）が得られる可能性があるため、\(d\)で最後の列を反転させることで、合法的な回転群\(SO(3)\)に強制的に戻す。\((\mathbf R,\mathbf t)\)が求まったら現在の推定値に累積し、収束するまで次の反復に進む。
なぜ重心減算 + SVDなのか？\(E\)を展開すると、重心減算後に並進項と回転項が分離される：最適な並進は変換後のソース重心をターゲット重心に一致させる必要があり、残りの回転項\(\sum_i\|\mathbf R\mathbf p_i'-\mathbf q_i'\|^2\)は\(\operatorname{tr}(\mathbf R^\top\mathbf H)\)を最大化することと等価であり、直交拘束の下でこのトレースは\(\mathbf H\)のSVDによって最適値が一括で求まる。これはまさに@sec-math_preliminaries の「SVDが最適な直交近似を与える」ことの直接的な実践である。
39.2 収束と精度
ICPに良好な初期値——ここでは単に単位行列を用いる（合成時の回転はわずか\(12°\)、並進は数ミリメートルであるため、単位行列は既に収束盆地内にある）——を与え、点間ICPを実行する。結果：58回の反復で収束、最終的な対応RMSは0.6443 mm、復元された回転角は\(10.69°\)（真値は\(12°\)、回転角誤差は\(1.97°\)）、並進誤差は\(0.233\,\text{mm}\)であった。図 39.2 には、位置合わせ後に赤いスキャン点群が青いモデルにぴったりと沿っている一方、右側の侵入壁は元の位置に孤立したままであることが示されている——対応の棄却によりこれが非重複部分と正しく判断され、変換推定を汚染することがなかった。
この\(0.6443\,\text{mm}\)という値は注目に値する：収束は正しい（姿勢は正しい）ものの、明らかに\(0.1\,\text{mm}\)のノイズフロアより上で停止している。その理由は点群の形状に隠されている——この点群は広大なコンベヤ平面に支配されており、点間メトリックは平面上に固有の弱点がある：ソース点がターゲット表面の接線方向に沿ってスライドしても、最も近いターゲット点までの距離はほとんど変化しないため、目的関数は接線方向でほぼ平坦になる。そのため各反復でわずかにしか移動できず、収束は遅く浅い極小値で停止しやすくなる。図 39.3 の赤い曲線はこの事実を明確に示している：RMSが急速に低下した後、長くほぼ平坦な尾を引き、58回の反復をかけてもプラトーに向かってゆっくりと這うだけで、ノイズフロアまで鮮やかに到達することはない。
終了判定基準：ICPは通常「連続する2ラウンド間のRMSの変化が閾値を下回る」または「最大反復回数に達する」と停止する。本章の実装ではRMSの変化が\(<10^{-5}\,\text{mm}\)になったら停止し、60回の反復上限を保険として設けている。緩すぎる判定基準では早期に終了し精度が不足する；厳しすぎる判定基準ではノイズフロア付近で空回りする——平面に支配された点群での点間ICPの長い尾は、まさに判定基準の調整が難しい典型的な場面である。
39.3 局所的極小値：ICPの弱点
ICPは局所収束しか保証しない——初期値に最も近い谷底に忠実に転がり落ちるが、それが大域的に最適な谷底であるとは保証しない。これを検証するため、意図的に初期値を悪くする：良好な初期値に加えて、さらに\(Z\)軸回りに\(40°\)回転させ、それ以外は変更せずに同じ点間ICPを再実行する。結果は衝撃的である：60回の反復を使い切り、最終的なRMSは2.0824 mm、回転角誤差は29.3°、並進誤差は8.20 mmであった。図 39.4 の緑色の「位置合わせ」結果は全体的にモデルからずれている——ICPは収束したが、誤った局所的極小値に収束したのである。図 39.3 の緑色の曲線は全体を通して高い位置に留まり、二度と低下しない：最近傍対応が最初から誤って一致し、誤った対応から誤った変換が解かれ、誤った変換が誤った対応を強化し、状況はますます悪化する。
これからICPの工学的応用における最初の鉄則が導かれる：ICPは精密化器であり、探索器ではない。「おおよそ正しい」姿勢をサブミリメートル精度まで磨き上げることはできるが、「大きくずれた」姿勢から正解を探し出す力はない。すべての局所的極小値にはそれぞれの収束盆地があり；初期値がどの盆地に入るかによって、ICPはどの谷底に転がり落ちるかが決まる；大域的最適解の盆地に入った場合に限り、結果が正しくなる。
そのため工業的な3次元位置決めではほぼ常に粗から精（coarse-to-fine）パイプラインが採用される：まず初期姿勢に感度の低い粗位置合わせアルゴリズム——チャプター 40 の特徴記述子または点対特徴（PPF）に基づくもの——を用いて、粗いが正しい盆地に入る初期値を求め、次にICPに渡して精密化を行う。粗位置合わせは「正しい盆地を見つける」役割を担い、ICPは「谷底まで転がり落ちる」役割を担い、それぞれが役割を果たす。
39.4 点対面バリアント
セクション 39.2 で明らかになった点間ICPの弱点——接線方向のスライドがペナルティ化されないため収束が遅い——には、適切な対処法がある：点対面バリアントである。これはソース点からターゲット点までの直線距離を最小化するのではなく、ソース点からターゲット点における接平面までの距離を最小化する。ターゲット点\(\mathbf q_i\)における表面法線を\(\mathbf n_i\)（ターゲット点群の各点について\(k\)近傍を取得してPCAを実行し、最小固有値に対応する固有ベクトルとして得られる）とすると、各ラウンドで次の問題を解く：
\[ \min_{\mathbf R,\mathbf t}\sum_{i}\Bigl(\bigl(\mathbf R\mathbf p_i+\mathbf t-\mathbf q_i\bigr)\cdot\mathbf n_i\Bigr)^2 . \]
法線方向のずれのみをペナルティ化し、接線方向のスライドは許容する——これはまさに点間ICPを停滞させていた束縛を緩めるものである。各ラウンドで小角度回転を\(\boldsymbol\alpha=(\alpha_x,\alpha_y,\alpha_z)\)として線形化し、並進と合わせて6次元の未知量とし、\(6\times6\)の正規方程式を解く。
効果は即座に現れる。同じ良好な初期値、同じデータで、点対面バリアントはわずか9回の反復でRMS 0.0991 mmに収束し、回転角誤差は\(0.016°\)、並進誤差は\(0.005\,\text{mm}\)であった——直接\(0.1\,\text{mm}\)のノイズフロアに到達したのに対し、点間ICPは58回の反復をかけても\(0.6443\,\text{mm}\)で停止したままである。図 39.3 の青い曲線は急激に低下し、数ステップで底に到達しており、赤い曲線の長い尾とはっきりと対照的である。
この9対58という差を直感的に理解する：平面領域では、点間ICPはソース点をターゲット表面上で「アイススケート」させるようなもので、各ステップで法線方向の距離をわずかに縮めるだけで、接線方向の自由度は無駄になっている；点対面ICPは接線方向の自由度を直接最適化器に戻すため、すべてのステップが（法線方向の）切り札として使われる。代償として、ターゲット点群の法線を計算する必要があり、法線の推定品質に感度が高くなる。より高速かつ高精度であるため、工業用ICP実装のほとんどはデフォルトで点対面を使用し、点間ICPは主に教育用ベースラインおよび法線が信頼できない場合の代替手段として機能する。
39.5 3D校正
位置合わせ（レジストレーション）の近縁の技術として3D校正（3D correction）がある：ワークを随意に置かれた姿勢から基準座標系へ「姿勢補正」する処理である。チャプター 5 では画素を物理座標系にキャリブレーションするのに対し、3D校正では測定されたワークの姿勢を設計基準に位置合わせする——平面度測定、傾き検査、後続の計測を行う前に、基準面を水平に調整する処理が先行することが多い。
最も一般的なものは平面校正である：本来水平に置かれるべきワークにわずかな傾きが生じている場合、これを回転補正して\(z=0\)基準面に一致させる。手順は単純である——ワーク表面の点群に対し最小二乗法による基準面フィッティング（least-squares plane fit）を行って法線\(\mathbf n\)を求め、\(\mathbf n\)を\(+Z\)軸に回転させる回転行列を算出し、点群全体に適用する。「単位ベクトル\(\mathbf n\)を\(\mathbf z\)に回転させる」処理には既知の構成法がある：回転軸を\(\mathbf n\times\mathbf z\)、回転角を2つのベクトルのなす角とし、ロドリゲスの公式に代入する。
実験として、\(7°\)の複合傾き（やや斜めの軸周り）を持つ平面にノイズを重畳した点群を作成した。法線をフィッティングして回転補正した結果、残留傾き角は\(6.9999°\)から\(0.0000°\)まで低下した——1回のフィッティングと1回の回転でワークの姿勢が完全に基準に一致した。図 39.5 はYZ側面視で前後の比較を示している：赤の傾斜した点帯が回転補正され、\(z=0\)参照線に沿う緑の平坦な点帯になっている。
さらに深く考察すると、校正は「理想的な姿勢」に対する位置合わせ（レジストレーション）である：位置合わせの対象が別の実測点群ではなく、解析的に定義された基準（ここでは\(z=0\)平面）である点が異なる。そのため最近傍探索の反復は不要であり——対象の幾何形状は閉形式で表されるため、1回のフィッティングで変換が得られる。この考え方を拡張すると、CADの基準面、基準軸、基準穴への位置合わせはすべて「理想的な幾何形状への位置合わせ」という同種の校正問題となる。
39.6 SciVision実装
本章の幾何学および数学的処理——kd木、Kabsch（SVD）、点間/点対面反復、平面フィッティングと回転補正——はすべて独自に実装しており、これこそが本章の核心である；SciVision SDKは点群のIOとクロスバリデーションによる裏付けという補助的な役割のみを担っている。この選択には実測に基づく根拠がある：使用したマシンのSDKの3D関数のうちいくつかは位置合わせ（レジストレーション）シナリオで信頼性が低く——Sci3DKdtree::CreateKdTreeの一部のオーバーロードはエラーコード121106105を返し、Sci3DAxisTransform::Transform3DPointArrayは0xC0000005でクラッシュする；唯一SciSv3DSurfaceCorrection::ApplyPlaneCorrectionが使用可能であり、本章では平面校正の結果の裏付けにこれを用いている。サンプルプロジェクトでは各SDKの呼び出しを独立したサブプロセスで実行し、潜在的なクラッシュを分離してメインフローの図の出力に影響を与えないようにしている。
最近傍探索 + 対応点除去（中央値しきい値）のメインループ：
for (size_t i = 0; i < src.size(); ++i) {
cur[i] = mul(res.R, src[i]) + res.t; // 現在の姿勢で元の点を変換
nn[i] = kd.nearest(cur[i], d2[i]); // 独自実装のkd木による最近傍探索
}
std::sort(sorted.begin(), sorted.end());
double med = sorted[sorted.size() / 2]; // 対応点距離の中央値
double thresh = std::max(med * 2.5, 0.5); // 中央値法による除去しきい値
// dist <= thresh の対応点のみをKabschに渡す——壁への侵入などの大きな距離の対応点は除外されるフィルタリング後の対応点をKabsch法に渡し、閉形式の剛体変換（重心除去 + 相互共分散行列のSVD + 行列式補正）を求める。これは@sec-icp-problem の数式に1行ごとに対応している：
Mat3 H = /* Σ p_i' q_i'^T */;
symEig3(mul(transpose(H), H), w, V); // H^T Hの固有値分解 → 右特異ベクトル
// Uの列 = H v_i / σ_i；σ_i = sqrt(λ_i)
Mat3 VUt = mul(V, transpose(U));
double d = det(VUt) < 0 ? -1.0 : 1.0; // 反射を防ぎ、det(R)=+1に強制
R = mul(mul(V, Mat3{{1,0,0, 0,1,0, 0,0,d}}), transpose(U));
t = ct - mul(R, cs);点対面バリアントでは各対応点を線形化して\(6\times6\)の正規方程式に組み込む。未知変数は小角度回転\(\boldsymbol\alpha\)と並進である：
double r = dot(s - q, nrm); // 点から目標接平面への符号付き距離
Vec3 c = cross(s, nrm); // 回転部分のヤコビアン
double a[6] = {c.x, c.y, c.z, nrm.x, nrm.y, nrm.z};
for (int u = 0; u < 6; ++u) { // 正規方程式 A x = b を累積
for (int v = 0; v < 6; ++v) A[u][v] += a[u] * a[v];
bb[u] += -r * a[u];
}平面校正ではfitPlane（最小二乗法による\(z=ax+by+c\)の解）で法線を求め、次にrotateVecToZ（回転軸\(\mathbf n\times\mathbf z\) + ロドリゲスの公式）で回転補正する——2つの独自実装関数であり、SciSv3DSurfaceCorrectionを裏付けとしている。完全な実行可能プロジェクトはcode/icp_registration/に格納されている。
産業事例：ロボットハンドリングの点群位置合わせ（レジストレーション）
ビンの中にピッキング対象の部品が散乱している状況で、ロボットは3Dカメラで点群をスキャンしてCADモデルに位置合わせ（レジストレーション）し、6DoF姿勢を算出してピッキング計画を立てる。初期の方案では純ICPを直接使用したが、ラインは頻繁に不具合を起こした——部品の初期姿勢が乱雑であるためICPが常に誤った局所的極小値に陥り、姿勢がずれてマニピュレータが目標を外したり把持に失敗したりした。改造の鍵は粗位置合わせ（レジストレーション）の工程を追加することであった：まず3Dマッチング（チャプター 40）の点対特徴を用いて粗い姿勢を算出し、次にICPに渡してサブミリメートル精度まで精緻化したところ、姿勢が安定した。別の避けられない落とし穴は非重複領域である：ビンの壁や隣接する部品が視野に入るが、これらの点には目標モデル上の対応点が存在しないため、対応点除去（距離しきい値）で1つずつ除外する必要がある。除外しなければ、これらの点が姿勢を外側に引きずりずらす原因となる。教訓はICPは精緻化器であり探索器ではない。常に粗位置合わせ（レジストレーション）を前に置いて初期値を供給しなければならないことである。
39.7 まとめ
- 位置合わせ（レジストレーション） = 2つの点群を一致させる剛体変換\((\mathbf R,\mathbf t)\)を求める処理であり、2D位置補正（チャプター 19）の3D版である；ICPは「最近傍探索による対応点抽出 → 遠方の対応点を除去 → SVDによる最適変換の算出 → 反復」のループを用いて、対応点と変換が互いに原因と結果になるというデッドロックを解消する。
- Kabsch / Umeyama法は閉形式の最適剛体変換を与える：重心を減算し、相互共分散行列\(\mathbf H\)を構成してそのSVDを求め、\(\mathbf R=\mathbf V\,\mathrm{diag}(1,1,d)\,\mathbf U^\top\)とする。ここで\(d\)は反射への縮退を防ぐための係数である——これは@sec-math_preliminaries のSVDの直接的な応用である。
- ICPは局所収束しか保証せず、初期値に強く依存する：良好な初期値の場合58回の反復で収束し、RMSは0.6443 mmとなる；不良な初期値（40°余分に回転した場合）では誤った局所的極小値に陥り、回転誤差は29.3°となる。産業現場では必ずcoarse-to-fineのアプローチを用いる——まず3Dマッチング（チャプター 40）による粗位置合わせ（レジストレーション）で初期値を供給し、次にICPで精緻化する。
- 点対面バリアントは点間バリアントよりはるかに高速である：法線方向のずれのみをペナルティとし、接線方向のすべりを許容するため、本章の実験では9回の反復でRMS 0.0991 mmに到達した（点間バリアントは58回の反復でも0.6443 mmのままであった）。そのため産業用ICPでは点対面バリアントが多く採用されている。
- 3D校正は「理想的な姿勢」に対する位置合わせ（レジストレーション）である：基準面をフィッティングして\(+Z\)軸に回転補正することで、7°の傾きを1ステップで0.0000°まで低減する；非重複領域（壁への侵入、ビンの壁など）は対応点除去で除外する必要があり、これは位置合わせ（レジストレーション）を安定して実用化するための前提条件である。
ICP、点群位置合わせ（レジストレーション）、3D姿勢推定についてより体系的な解説は、Stegerらの著作(Steger, Ulrich, と Wiedemann 2018)を参照されたい。ICPアルゴリズム自体には2つの古典的な起源がある：BeslとMcKayは最近点反復に基づく点間の定式化を提案し(Besl と McKay 1992)、ChenとMedioniは複数の距離画像を位置合わせ（レジストレーション）する際に収束の速い点対面の誤差評価指標を提案した(Chen と Medioni 1992)——これらはまさに本章の実験における58回と9回の反復の差に対応する；各種サンプリング、対応付け、除去のバリアントの体系的な比較についてはRusinkiewiczとLevoyのサーベイを参照されたい(Rusinkiewicz と Levoy 2001)。本章の各反復における閉形式の最適剛体変換は、重心除去後の相互共分散行列のSVDによって得られる（Kabsch/Umeyama法）が、その最も初期の厳密な導出はArun、Huang、Blosteinの論文に記載されている(Arun, Huang, と Blostein 1987)。