Tulip Visionにおける3D治具のバンドル調整最適化

Tulip ビジョンの3D治具

Tulip 3Dマーカー治具を使用しているのは、現場のような非常に騒がしい環境でも、物体を忠実に追跡できる最も正確な方法だからです。治具マーカーは簡単に貼り付けることができます。ビンに貼り付けたり、フォークリフトに貼り付けたり、セミトラックに貼り付けたり、小さな時計工具に貼り付けたりできます。冶具の古典的な使用例は、当然、ワークピースをはめ込む冶具オブジェクトを追跡することですが、3D機能を使えば、オブジェクトのさまざまな表面にマーカーを貼り付けて、複雑なオブジェクトを追跡する方法が加わります。

治具は、多数の3Dマーカーをグループ化したものです。3Dマーカーは、物体が剛体であると仮定した場合、物体の「フレーム」（フレームとは、位置と向きを定義するための3つの軸、X軸、Y軸、Z軸のこと）に固定された3D物体上の位置をピンポイントで示します。マーカーを見るたびに、それがオブジェクトの同じポイントに固定されていることがわかります。治具マーカーグループ - 完全なオブジェクトを定義します。治具は複雑な 3D 形状を定義することができ、マーカの一部は表示され、一部は非表示になります。表示されているマーカーは、オブジェクトの位置と方向を見つけるのに役立ちます。非表示のマーカーが表示されると、非表示になった他のマーカーを補うことができます。

マーカーから治具を定義する場合、カメラに対して異なる角度からマークされたオブジェクトの多くのビューをキャプチャします。カメラでキャプチャされた各フレームは、マーカーが付いたオブジェクトの別の「ビュー」を提供します。そして各フレームで、オブジェクト上のマーカーの位置とその関係（変換）を計算します。十分なビューをキャプチャした後、それらを組み合わせてオブジェクトの全体的な3Dモデルを作成します。しかし、光学や計算の数値的安定性に関係するいくつかの理由により、対象物周辺のビューは常に完全に一致するわけではありません。実際、対象物のビューを撮れば撮るほど、レジストレーションの累積誤差は大きくなり、最終的なレジストレーションが無駄になることもあります。そこで登場するのが "バンドル調整 "です。

バンドル調整（BA）は、形状を再構成するために複数のカメラビューを登録することによる累積誤差に対抗する数値最適化プロセスです。従来のBAでは、カメラの光学モデリングを含め、ほとんどの場合、再構成のすべてのパラメータを最適化します。しかし、BAのプロセスを説明する前に、最適化を必要とするパラメータを定義する必要があります。Richard Szeliski教授の素晴らしい本"Computer Vision：Algorithms andApplications", Springer press, 2011 (chapter 7 pp320).

カメラが平面オブジェクトであるマーカーを見ている場合、カメラの原点に対するマーカーの向きを計算することができます。次の図を見てください：

マーカーはカメラビューに表示され、画像平面に投影されます。これは、3Dと2Dのピクセル座標間の変換を定式化するのに役立つ概念的な構造です。しかし、マーカーがあるシーンを撮影したとき、私たちは3D点のパラメータを知りません。この投影は次の式で表されます：

X、Y、Z はマーカーの中心などの 3D 座標で、x、y は画像上の角の 2D ピクセル位置です。これは、カメラ中心と実際の3D点（図中の薄くなったオレンジ色の点を参照）からの光線上のどこにでも、世界の3D点が現れる可能性があるという事実によって引き起こされる、パラメータの曖昧さ、情報の欠落を示します。言い換えれば、任意の縮尺の物体が任意の大きさで画像上に現れる可能性があり、すべてはカメラからの距離に依存します。また、一般性を損なわない範囲で、物体の3D回転（rパラメータ）と平行移動（tパラメータ）、またはカメラの逆方向の回転（fパラメータ）と平行移動（cパラメータ）もこの式に含まれます。fパラメータとcパラメータはカメラの光学系をモデル化する「固有パラメータ」です（このおもちゃの例では非常に大雑把に）。

とはいえ、3D 点と 2D 点の間には線形関係があり、この方程式のパラメータがすべてわかっていれば、(1) 2D ピクセル座標からマーカーの現実世界の 3D 位置、(2) カメラに対するマーカーの回転 ri と平行移動 tx,y,z を計算できます。2D 座標を扱うには、式中のパラメータを単純に取り除くことはできないことに注意してください。実際、ピクセル ポイントを取得するには、ベクトルの最後のエントリで除算します。

2Dから3Dへの十分な対応点があれば、上記の方程式をRとtを復元できるような（均質な）一次方程式のセットに並べ替えることができます。2次元点は画像を見てコーナーを見つけることで得られます。3D点はマーカーの配置から得られますが、これも（マーカーを印刷したので）私たちのコントロール下にあります。ポーズを復元する全体的なプロセスはPerspective-n-Pointとして知られており、その解法には多くのアプローチやアルゴリズムが存在します。例えば、OpenCVを使ってPythonで、2D-3Dの整列された点の集合からカメラのポーズを求める方法です：

 _, R, t = cv2.solvePnP(aligned_3d, aligned_2d, K, dc)

最適化問題

最後の「投影」操作を次のように注釈してみましょう：

$$P_{2D}=\mathrm{Proj}([R|t], P_{3D})$$

つまり、3D点P3Dとカメラとオブジェクト間の回転Rと平行移動tを投影して、2Dピクセル位置（P2D）を得ます。この投影法の主な問題点は、ピクセル座標の2D点の計算に基づいていることです。物体の姿勢[R|t]を求めた後、3D点を再投影（画像上の2Dに投影）すると、多くの場合、画像上の位置からオフセットされた2D位置が見つかります。次の図は、カメラに対して強い角度があったり、ぼかしがあるような極端な状況で、通常よりコントラストが強く発生するオフセットを示しています。

私たちの目標は、これらすべての2次元オフセットが可能な限り小さくなるようなカメラ位置パラメータを見つけることです。これを数式で表すと、残差を最小化する最適な[R|t]を探す以下の最小化問題を解きたいと考えます：

\Γ Γ Γ\sum_i \Vert \mathrm{Proj}([R|t],P_i^{\mathrm{3D}}) - P_i^{\mathrm{2D}}\Vert^2

再投影された3次元点と2次元点の差を残差と呼びます。残差を2乗するので、一般にこの問題を最小2乗問題と呼びます。Proj(.)演算子は非線形なので、この特殊なケースは非線形最小二乗問題です。この定式化により、最適化問題にカメラの固有パラメータなどを導入し、それらの最適値を求めることもできます：

\hat{[R|t]},\hat{\{P^\mathrm{3D}\}},\hat{K} = \mathop{\arg\min}_{[R|t],\{P^\mathrm{3D}\},K}\sum_i\Vert^2

これは、OpenCVを使ってPythonで、対応する2D-3D点のペアから残差を計算し、残差のリストを出力する例です：

def calcResiduals(Rt)：
 projPts2d,_ = cv2.projectPoints(pts3d, Rt[:3], Rt[3:], K, None).
 return (np.squeeze(projPts2d2) - pts2d21).ravel().

幸運なことに、非線形最小二乗問題を解くためのアルゴリズムやソフトウェアパッケージは、Ceres Solver、MATLABの様々な手法、PythonのSciPyなど、数多くあります。例えば、SciPyとOpenCVを使えば、次のように問題を解くことができます：

res = scipy.optimize.least_squares(calcResiduals、 
 np.hstack([
 cv2.Rodrigues(R)[0]、 
 t[np.newaxis].
 ]).ravel())

これまで一般的な用語でBAについて説明してきましたが、3D治具の最適化の目標は少し異なります。私たちが3D治具を作るとき、要するに3Dマップを作ることになります。マッピング（およびローカライゼーション）は、自律航法やオドメトリーなどではよく知られた問題です。私たちの治具マッピング技術は、SLAM（Simultaneous Localization and Mapping）アルゴリズムに似ており、観察された世界のマップを段階的に構築し、様々な線形推定アルゴリズムからの残留誤差を低減するために、時折BAを実行します。

最初のセクションで述べたように、あるフレームでは、あるマーカーは見えても他のマーカーは見えないことがあります。マッピングが進むにつれて、マーカー同士の位置関係を知る手がかりが増えます。最初に見えるマーカーから始め、その3D構造に注目します。たとえば、マーカー 1 とマーカー 2 の間の変換は T12 と記録します。後のフレームでは、マーカー 1 は見えなくなり、マーカー 3 が現れます。2 から 3 への変換を T23 で記録し、1 から 3 への変換を連結して記録します：T13 = T12T23。

マッピング処理では、先に説明したマーカーの3Dポーズを復元する際の本質的な誤差に加え、マップにさらなる誤差が生じます。変換の連結は誤差を複雑化させ、退化したケースが発生するほどです。この複合エラーを緩和するためにBAを適用しなければなりません。

最適化の1つの選択肢は、カメラのポーズ推定から得られる変換を固定することであり、これは先ほどのBA定式化と似ています。私たちは、3D点が与えられたときの残差を最小化するCamPを探しています：

\．\P_i^{mathmrm{CamP}} - P_i^{mathmrm{CamP\Vert^2

しかし、カメラのポーズは（2D-3D対応を通じて）3D点から導出されることに注意してください。そのため、3D点の座標自体を最適化し、そこからカメラポーズを再計算することも可能です。カメラポーズを固定し、3D点に基づいて最小化し、2D再投影残差を最小化する最適な3D点を探します：

\hat{\{P^{\mathrm{3D}}\}} = \mathop{\arg\min}_{\{P^{\mathrm{3D}}\}}\P_i\Vert^2

このトリックは主に、オブジェクト上にあり、画像から生成された2D点に対する誤差が最小となる最適な3D点のセットを得るのに役立ちます。私たちは3DマップポイントとそのマーカーIDの関係を保持し、実行時に2D-3D対応を見つけて、solvePnPでオブジェクトのポーズを復元できるようにしています。新しい入力フレームでは、マーカーコーナーの2D位置を特定し、マップの3D点とマッチングさせます。これにより、全体として、多くの2D-3D点から一緒にオブジェクトのポーズを見つけ、誤差を平均化します。

3DジグマップでBAを実行した後、2Dオフセットが減少し、オブジェクトのポーズ推定がはるかに良くなることがはっきりとわかります。

Tulip Visionの治具は、現場での作業をセンシングするための幅広いユースケースを提供します。新しい3D治具機能により、手持ち工具のように異なる角度から見える複雑な工具のトラッキングなど、新しいユースケースが可能になります。治具マッピングとバンドル調整を使用することで、最小限の誤差と最適化されたジオメトリで複雑なオブジェクトマップを作成することができます。ジグは、最適化が組み込まれた状態で、すぐにTulip 使用できます。ツールやワークステーション機器、さらには材料のトラッキングにご利用ください。