40  3D マッチング

ICP（チャプター 39）は「すでに大まかに位置合わせされた2つの点群をサブピクセル精度で一致させる」ことを解決する——それは精緻化を行う機構であるが、生来の近視眼である：初期値が数十度ずれていると、誤った局所的最小値にはまり込み、二度と抜け出せなくなる。しかし生産ライン上のカメラが捉えるのは、孤立した点群の塊ではなく、常に煩雑なシーンの一場面である：地面、料箱の壁、隣接するワーク、ノイズ——探し出すべき部品はその中に隠れており、その姿勢は完全に未知である。3D マッチング（3D matching） が担当するのはまさにこの「無から有を生み出す」段階である——煩雑なシーンの点群内で既知のモデルを位置特定し、その大まかな 6 自由度（6 degrees of freedom, 6DoF）姿勢（pose） を求めた後、この粗い姿勢をICPに渡して精緻化を行う。それは2Dのテンプレートマッチング（template matching）（チャプター 16）と形状マッチング（shape matching）（チャプター 17）の3次元世界における対応物であり、ロボットによる把持と組立の目でもある：部品の「どこにあるか、どちらを向いているか」がわかって初めて、アームは把持に取りかかれるのである。

本章のモデルは非対称なL形ブラケットであり、約5400個の表面点を持つ：長腕と短腕でL字形を構成し、長腕の先端に凸台が追加されている（図 40.1 (a)）——この凸台はL字形自体に残る対称性を意図的に崩し、主軸方向を一意に特定可能にするためのものであり、その理由は セクション 40.2 に記載する。シーン（図 40.1 (b)）にはこのブラケットのインスタンスが2つ含まれており、それぞれ異なる6DoF姿勢をとる：インスタンス1は清浄で完全な状態、インスタンス2はオクルージョン（表面の約1/3が欠落）を受けている；さらに地面となる平面、外乱となる円柱が存在し、シーン全体に\(\sigma=0.1\,\text{mm}\)のガウシアンノイズが重畳されている。課題は、円柱をブラケットと誤認することなく、2つのインスタンスをいずれも発見し、それらの姿勢を正確に求めることである。

(a) モデル：非対称L形ブラケットのテンプレート（高さ疑似カラー）、長腕＋短腕＋頂部の凸台
(b) シーン：青色の地面＋完全なL字（左、オレンジ）＋オクルージョンを受けたL字（右上、一部欠落）＋外乱となる円柱（赤）、\(\sigma=0.1\,\text{mm}\)のノイズを含む
図 40.1: 本章の実験データ。直交俯瞰視点、高さ疑似カラーでレンダリングしている。モデルは約5400点；シーンには異なる姿勢の2つのインスタンス（一方はオクルージョンあり）、地面となる平面、外乱となる円柱が含まれる。

40.1 6 自由度位姿探索

2Dマッチングで探索されるのは3つのパラメータ\((x, y, \theta)\)、すなわち平面平行移動に面内回転を加えたものである。前章までのテンプレート・形状マッチングが角度×スケール格子上で各セルごとに総当たりでスコアリングできたのは、探索空間がたった3次元であり、画像ピラミッドが探索空間を段階的に縮小してくれたからに他ならない。3次元に話を移すと、剛体の姿勢には6つの自由度が存在する。3つの平行移動成分\((t_x, t_y, t_z)\)に、3軸周りの回転成分3つを加えたものである。次元が3から6に上がると、探索空間は2倍になるどころか指数関数的に爆発する。各回転軸を\(K\)段階に離散化し、各平行移動軸も\(K\)段階に離散化すると、候補の総数は\(K^6\)となる。チャプター 16 で「総当たりスコアリングでは耐えられず、ピラミッドが必須である」とされた動機は、3Dでは回避不能なほどに増幅される。各軸を\(30^\circ\)/\(10\,\text{mm}\)と粗く設定しても、\(K\!\approx\!12\)ならば百万単位の候補が生まれ、各候補について数千点の点群で最近傍によるスコアリングを行う必要がある。

6DoFの「6」とは、剛体が3次元空間で持つ全自由度であり、平行移動3つと回転3つを合わせたものである。部品にスケーリングの可能性がある場合（物距離が較正されていない場合）は、スケールの自由度が1つ追加される。ただし工業用3Dカメラはすでにミリメートル単位で較正されており、スケールは通常固定されるため、この点は3Dマッチングが2Dマッチングより考慮しなくて済む点である。

結論は2Dの場合と同じであるが、その必要性はより切実である。総当たりは使えず、粗密（coarse-to-fine）方式が必須である。まず低コストな手法でシーン内から「部品である可能性のある」少数の候補を抽出し、それぞれに粗い姿勢を与える（粗マッチング coarse matching）。次にそれら少数の候補に対してのみICPを実行し、姿勢をノイズレベルまで精密化する（精マッチング fine registration）。粗マッチングは\(K^6\)の探索空間を少数の仮説に縮約する役割を担い、精マッチングは各仮説の数度・数ミリメートルの誤差を消し去る役割を担う。両者は欠かせないものであり、このパイプラインが本章の主軸となる。

40.2 粗マッチング手法

粗マッチングの最初の手順はマッチングではなくシーンセグメンテーションである：1フレームの点群を「個々の候補物体」に切り分ける処理である。点群前処理（チャプター 38）の2つの定番手法を再利用する——まずRANSACで最大平面をフィッティングし、地面を除去する（地面はシーン内で点数が最も多い平面であり、ランダムに3点をサンプリングしてフィッティングし、インライアを計数し、200回の反復で最良の結果を採用する）。地面が除去されると、その上に浮かんだ物体同士が分離されるため、次にユークリッドクラスタリング（Euclidean clustering）を実施する：空間をボクセル化し、隣接するボクセル内の点で距離が閾値（本章では\(4\,\text{mm}\)）未満のものをUnion-Find構造で併合し、各連結成分を1つのクラスタとする。本章のシーンでは地面除去後、明確に3つのクラスタ（2つのブラケットのインスタンス＋1つの円柱）に分かれる。

各クラスタには粗姿勢を求める必要がある。最も単純な手法は主成分分析（principal component analysis, PCA）である：クラスタの点の重心を並進の初期値とし、共分散行列の3つの固有ベクトルを物体の3つの主軸として、モデル自身の主軸と位置合わせすることで回転の初期値を得る。原理は明快である——細長いL形ブラケットの場合、点群の最大分散方向は必ず長腕に沿うため、PCAはこの軸を一目で「認識」できる。ただしPCAには2つの本質的な制限が存在する：1つ目は主軸に符号と順序の曖昧さが存在することである——固有ベクトルの正負は任意であり、固有値がほぼ等しい場合には順序も変動しうるため、3軸の右手座標系の組み合わせは計\(24\)通りが「同等に妥当」となり、これらを1つずつ試して「どの組み合わせがモデルをシーンに最もフィットさせるか」で曖昧さを解消する必要がある。2つ目はオクルージョンに敏感なことである——部品の一部が欠落すると点群の分散分布が変化し、主軸もそれに伴って傾くため、PCAが出力する回転は数十度から百度単位で誤る可能性がある。後者はまさにインスタンス2の事例である（セクション 40.3）。

PCAの他に、より強力な粗マッチングのクラスが存在する：点対特徴（point-pair feature, PPF）である——モデル上の点の対について、それらの距離と2点の法線同士のなす角から4次元特徴を構成し、姿勢を投票で決定する手法である。オクルージョンや煩雑さに対してよりロバストであり、Drostらによる工業表面マッチングの基礎をなす。本章ではPCA＋複数候補探索で原理を示す；PPFは実装量が大幅に多いため、発展課題として残す。

本章では各クラスタに対して全\(24\)通りのPCA候補姿勢を生成し、それぞれについて捕捉半径の広い高速ICPを実行し、シーン被覆度（次節で定義）で最良のものを選択する——安価な「試行錯誤探索」を1回行うことで、PCAによる単一推定の脆弱性を補う。

40.3 粗→精パイプライン

本章の核心的教育実験である：粗マッチングが初期値を供給し、ICPがノイズフロアを押し下げる、2つのインスタンスがこのパイプラインの2つの典型的な筋書きを余すところなく演じている。

インスタンス1（クリーン）：表面が完全で点群が密であるため、PCAの主軸はほとんど偏らず、勝利候補の粗姿勢はすでに回転誤差 \(0.02^\circ\)、並進誤差 \(0.01\,\text{mm}\)——全サンプリング下でPCAはほぼ一歩で目標に到達する。ICPが引き継いだ後、これは\(0.01^\circ\)/\(0.00\,\text{mm}\)、インライア残差 \(0.162\,\text{mm}\) に精化され、シーンカバレッジとモデルカバレッジの両方が \(100\%\) に達する。粗姿勢がすでにノイズフロア付近にあるため、ここでICPはほとんど行うことがない。

インスタンス2（オクルージョンあり）：表面の約1/3を除去した後、主軸は大幅に偏り、\(24\) 個の候補の中で最良の粗姿勢は回転 \(100.5^\circ\)、並進 \(21.8\,\text{mm}\) までずれている——この数値だけを見ると、PCAはほぼ失敗している。しかし、それでもモデルを正しいクラスター、正しいおおよその位置に配置しており、ICPが広い捕捉半径の下で最近傍を段階的に絞り込み、モデルを少しずつ「ねじ戻す」のに十分である。最終的にICPは\(0.38^\circ\)/\(0.90\,\text{mm}\)、残差 \(0.400\,\text{mm}\) に収束する。2段階にわたる削減は極めて顕著である：回転 \(100.5^\circ\!\to\!0.38^\circ\)（約 \(\times 266\)）、並進 \(21.8\!\to\!0.9\,\text{mm}\)（約 \(\times 24\)）。

クリーンインスタンスの残差 \(0.162\,\text{mm}\) はちょうどノイズフロア付近に位置する：各軸独立に \(\sigma=0.1\,\text{mm}\) の点の、真の表面までのRMSは約 \(\sigma\sqrt{3}\approx 0.17\,\text{mm}\) である。ICPがこの桁に収束した時点で限界であり——それより小さくなるとノイズへの過学習となる。オクルージョンありインスタンスの \(0.40\,\text{mm}\) はやや高いが、これは部分データ下での限界であり、アルゴリズムの欠陥ではない。

図 40.2 は2つのICP収束軌跡を重ねて描いている（縦軸の残差は \(\log\) スケール）。オレンジ線はオクルージョンありインスタンスである：粗姿勢に対応する約 \(4\,\text{mm}\) の残差から始まり、反復に伴って約 \(0.4\,\text{mm}\) まで段階的に減少する——各段階は、捕捉半径が絞り込まれて正しい対応がより多く取り込まれ、誤った対応が除去された結果である。青線はクリーンインスタンスである：最初から底に張り付いてほぼ水平である——最初からノイズフロアにあり、ICPは移動ではなく確認を行うだけである。この図は チャプター 39 の「ICPには良好な初期値が必要である」という主張に対する決定的な注脚となっている：良好な初期値（クリーン）はICPに行うことを残さず、劣悪な初期値（オクルージョンあり）はICPに実力を発揮させ、限りなく劣悪な初期値はICPを直接失敗させる——本章では「マルチ候補＋広い半径」により、インスタンス2の初期値をICPが到達可能な範囲に救い上げている。

図 40.2: ICP収束軌跡（横軸は反復回数、縦軸はインライア残差の \(\log\) スケール、グリッド線は \(0.1/1/10\,\text{mm}\)）。オレンジ＝オクルージョンありインスタンス、約 \(4\,\text{mm}\) から約 \(0.4\,\text{mm}\) まで段階的に減少；青＝クリーンインスタンス、終始ノイズフロア（約 \(0.16\,\text{mm}\)）に張り付いてほぼ水平。

40.4 オクルージョンと被覆度

チャプター 17 では2Dオクルージョンによってスコアが可視領域の割合に応じて低下するが、3Dマッチングでは「被覆」を向きの逆な2種類の被覆度（coverage）に分割する必要があり、それぞれ異なる事象を計測する。

  • シーン被覆度（scene coverage）：候補姿勢にモデルを配置した後、クラスタ内の点のうちモデル表面近傍に位置する点の割合である。「発見したこの点群のうち、モデルで説明できる点はどれだけあるか」を問う指標である。オクルージョンはシーンの点のみを除去し、残りの点は依然としてモデル上に位置するため、シーン被覆度はオクルージョンに対して非感受性である。この特性から、シーン被覆度は受け入れ判定基準として選択される：被覆度が高ければこのマッチングを受け入れる。
  • モデル被覆度（model coverage）：逆に、モデルの点のうち対応するシーンの点が見つかった点の割合である。部品のオクルージョンを受けた部分にはシーンの点が存在しないため、これらのモデルの点は「対応先がない」状態となり、モデル被覆度はオクルージョンに伴って直接低下する。この特性から、モデル被覆度はオクルージョンの重症度を計測するのに適している。

実験により、これら2つの指標の役割分担は明確に示されている。表面の約1/3がオクルージョンにより除去された後：モデル被覆度はオクルージョンのない状態の \(100\%\) から \(46.5\%\) まで低下し（ほぼ半減しており、オクルージョン量を忠実に反映している）、一方でシーン被覆度は \(100\%\) から \(79.3\%\) まで低下するにとどまる（依然として受け入れ閾値を大幅に上回っている）。図 40.3 の青色のオクルージョンありインスタンスは、幾何形状の一部が欠けていることが目視で明らかであるが、残りの点は依然としてモデルに隙間なくフィットしている。これはまさに「シーン被覆度が高く、モデル被覆度が低い」状態の一例である。仮にモデル被覆度を受け入れ判定基準とした場合、オクルージョンありインスタンスは不当に拒否されてしまうが、シーン被覆度を用いればスムーズに合格する。

外乱となる円柱は反例である：その幾何形状はL字ブラケットとは無関係であり、モデルをどのように配置してもクラスタの点がモデル表面にフィットすることはなく、シーン被覆度は約 \(5.4\%\) にすぎないため、受け入れ閾値を大幅に下回り、明確に除外される。被覆度による判定基準は、オクルージョンを受けた真の部品を受け入れつつ、形状の一致しない偽の目標を排除する必要があり、円柱は後者の判定の試金石となる。

図 40.3: シーンセグメンテーション結果：灰色=地面（RANSACにより既に標識されている）、赤/青/緑の3クラスタはユークリッドクラスタリングの出力——赤は完全なL字、青はオクルージョンを受けたL字（長腕の幾何形状の一部が明らかに欠けている）、緑は外乱となる円柱。

図 40.4 は最終的なマッチング結果である：復元された2つの姿勢でモデルをシーンに重ね合わせ、クリーンインスタンスは赤色、オクルージョンありインスタンスはマゼンタ色で表示されており、いずれも輪郭が隙間なく一致している。円柱は被覆度が不足しているため重ね合わされていない。オクルージョンありインスタンスの位置推定が成功したのは、「PCA単体では不十分であり、単純なICP単体でも不十分であるが、両者を連携させることで十分となる」という相乗効果によるものである。これは チャプター 17 の2D形状マッチングにおける「スコアが可視領域の割合に応じて低下し、minScoreが許容可能なオクルージョン割合となる」という論理と一脈相通じるものであり、3Dでは「1つのスコア」が「2種類の被覆度」に分割されている点が異なる。

図 40.4: マッチング結果：復元された2つの6DoF姿勢でモデルを灰色のシーンに重ね合わせ、クリーンインスタンスは赤色、オクルージョンありインスタンスはマゼンタ色で表示されており、いずれも一致している。外乱となる円柱は被覆度が不足しているため重ね合わされていない。

40.5 SciVision の実装

慣例に従い、まずSDKの実測結果を正直に記録する。SciVisionには2系統の3Dマッチングインターフェースが用意されている：SciSv3DSurfaceMatch（CreateSurface3DModel/FindSurface3DImageModel、距離画像表面マッチング）とSciSv3DPointsMatching（GetTransformationICP/GetTransformationPCA、点群位置合わせ）である。本章ではサブプロセスプローブを介してそれぞれを呼び出したところ、両方とも不活性に失効した：戻り値はrc=0（エラーもクラッシュも発生しない）であるものの、表面マッチングでは0個のマッチングが検出され、点群ICP/PCAでは既知の並進が\((0,0,0)\)として復元され、さらにDLLからstderrに"Directory does not exist."というメッセージが出力された——これは本書Part IXの複数の3Dモジュールと同じ特徴的な挙動であり（実行時リソースディレクトリが欠落している疑いがある）。SDKが使用不能であるため、本章の内容は手書きパイプラインによって担われている。

手書きパイプラインは4段階からなり、主要なコード片は以下の通りである。

// 1) RANSACによる地面除去：3点をランダムに選んで平面を200回フィッティングし、インライアが最も多いものを選択
std::vector<char> ground = ransacPlane(scene, /*tol*/ 0.4);

// 2) ユークリッドクラスタリング：ボクセル + Union-Find、tol=4mm、200点未満の小クラスタは破棄
std::vector<std::vector<int>> clusters = cluster(rest, 4.0, 200);

// 3) クラスタごとに：PCAで重心と主軸を求め、24個の符号付き置換候補のそれぞれに対して
//    広半径の高速ICPを実行し、シーン被覆度で最適なものを選択（曖昧さ解消 + 傾いた主軸の救済）
pca(clu, cs, Vs);                       // クラスタ重心cs、主軸Vs
for (auto& P : perms /*24個の右手系置換*/) {
    matMul(Vs, P.data(), VsP); matMul(VsP, VmT, Rc);   // 候補回転
    icp(modelScreen, clu, Rt, tt, 25, 14.0, 2.5);      // 広捕捉半径によるスクリーニング
    double sc = sceneCov(model, Rt, tt, cluDown, 0.6, rms);
    if (sc > bestSC) { /* 最適候補を記録 */ }
}

// 4) 最適候補を初期値として完全なICPを実行（Kabschの閉形式解、3×3 SVDはJacobi法で実装）
best.resid = icp(modelDown, clu, R, t, /*iters*/ 60, /*d0*/ 10.0, /*d1*/ 0.8);
best.sceneScore = sceneCov(model, best.R, best.t, cluDown, 0.5, rmsS);
best.ok = (best.sceneScore > 60.0 && best.resid < 1.5);   // 被覆度 + 残差の二重判定基準

ICPの各反復ではKabsch法を用いて最適な剛体変換を求める：対応点から重心を減算し、共分散行列\(C\)を累積し、\(C^{\mathsf T}C\)に対してJacobi法による固有値分解を実行して\(3\times 3\)のSVDを実現し、\(R=UV^{\mathsf T}\)から回転行列を得る（反射成分を補正して\(\det R=+1\)を保証する）。並進は2つの重心の差から求められる。捕捉半径は\(d_0\)から\(d_1\)へ線形に狭められるが、これはまさに 図 40.2 のオレンジ線が段階的に下降する理由である。一連の数学処理は完全に自己完結している——これは チャプター 37 および本書の一貫した観察と呼応するものである：点群アルゴリズム（ICP/Kabsch/PCA/PPF/kd木）の多くは自前で開発するか、専用ライブラリに依存する必要があり、SDKは3Dマッチングにおいては多くの場合、入出力と交差検証にしか役に立たない。産業用3Dマッチングを深く追求すると、自前開発はほぼ常態となる。

40.6 産業事例

産業事例：ランダムに積み重ねられた部品のビンピッキング

ビン内に同種の部品がランダムに積み重ねられており（ビンピッキング）、マニピュレータは各部品の姿勢を順に求めて把持して取り出す必要がある。3Dカメラで1フレームの点群をスキャンし、本章と同じフローで処理を行う：ビンの底面の平面を除去し、ユークリッドクラスタリングで候補を1つずつ切り出し、各クラスタに対してPCAによる粗姿勢の推定とICPによる精密化を行う。難点は積み重ねによる相互オクルージョンである——上部の部品が下部の部品を押さえつけるため、ほとんどのインスタンスのモデル被覆度は非常に低く、モデル被覆度に基づいて受理すると把持漏れが大量に発生する。対策はまさに本章で述べた判定基準の分離である：シーン被覆度に基づいて受理し（オクルージョンに耐性を持たせ）、次に把持可能面の可視性に基づいてソートし、最も完全な形状を持ち、グリッパが安定して接触できる部品から優先的に把持する。ビンの壁や隣接する部品による干渉は、被覆度の閾値で排除する——形状が一致しない点はモデルにフィットしないため、本章の円柱が\(5.4\%\)の被覆度で除外されたのと同じ原理である。得られた教訓は1つの言葉に凝縮される：3Dマッチングのロバスト性 = セグメンテーションの品質 × 被覆度判定基準であり、オクルージョン下での生産現場の法則は「最もよく見えるものから先に把持する」である。

40.7 まとめ

  • 3DマッチングはICPの前段階である：ICP（チャプター 39）は精密化を行うだけで良好な初期値を必要とする；3Dマッチングは乱雑なシーン内で既知のモデルを定位し、ICPに供給する粗い6DoF姿勢を出力する。これは2Dテンプレート/形状マッチング（チャプター 16、チャプター 17）の3次元版に相当する。
  • 6自由度により全探索が爆発的に増大する（\(K^6\)）ため、coarse-to-fine（粗から精への段階的処理）が必須である：粗マッチング（RANSACによる地面除去 + ユークリッドクラスタリング + PCAによる主軸・重心、\(24\) 候補で曖昧性を解消）により探索が少数の仮説に絞り込まれ、ICPにより各仮説がノイズレベルまで精密化される。
  • 粗→精の連携の実測値：クリーンなインスタンスではPCAの段階ですでに姿勢が高精度に近く（\(0.02^\circ\)/\(0.01\,\text{mm}\)）、ICPにより\(0.01^\circ\)/\(0.00\,\text{mm}\)まで改善される；オクルージョンのあるインスタンスでは粗姿勢が\(100.5^\circ\)ずれていたが、広い半径を持つICPにより\(0.38^\circ\)/\(0.90\,\text{mm}\)まで回復した（回転で\(\times 266\)、並進で\(\times 24\)の改善）。PCA単独と朴素なICP単独のいずれも不十分であり、両者の連携により十分な精度が得られる。
  • 2種類の被覆度の役割分担：シーン被覆度（検出された点の割合、オクルージョンに耐性あり）は受理判定基準として機能し、モデル被覆度（モデルが説明された割合）はオクルージョンの深刻度として機能する。オクルージョンによりモデル被覆度は\(100\%\!\to\!46.5\%\)に低下するのに対し、シーン被覆度は\(100\%\!\to\!79.3\%\)の低下にとどまる；干渉する円柱は\(5.4\%\)のシーン被覆度で除外される。
  • SDKの実態：SciSv3DSurfaceMatchとSciSv3DPointsMatchingはいずれも不活性に失敗し（rc=0、マッチングが0件またはゼロ姿勢が復元される）、本章は手動で構築したパイプライン（RANSAC + クラスタリング + PCA + Kabsch ICP）で処理を実装した——産業用3Dマッチングには独自開発または専用ライブラリが必要となることが多い。

3Dマッチングの原理、ならびに点対特徴と表面マッチングのエンジニアリング的実践は、Stegerらの著作(Steger, Ulrich, と Wiedemann 2018)で系統的に論じられている。局所的な幾何情報を記述子に符号化した後に投票で姿勢を求める手法は、この系統の手法に共通する考え方である：Drostらは点対特徴（PPF）を用いて大域的にモデリングし局所的にマッチングを行っており、これは産業用表面マッチングの代表的な手法である(Drost ほか 2010)；それ以前にJohnsonとHebertはスピンイメージを用いて乱雑なシーン内の3次元物体を認識している(Johnson と Hebert 1999)；またRusuらが提案した高速点特徴ヒストグラム（FPFH）は点群の粗レジストレーションで最も広く用いられる記述子の1つであり(Rusu, Blodow, と Beetz 2009)、本章のPCA以外に、よりロバストな粗姿勢の供給源となり得る。