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

Tulip Visionの3D治具

Tulip 3DマーカTulip 、製造現場のようなノイズの多い環境において、対象物を高精度で追跡する上で、これが最も確実な方法だからです。ジグマーカーは物体に簡単に貼り付けることができます。 コンテナ、フォークリフト、大型トラック、あるいは時計職人の小さな工具など、カメラが認識できる限り、あらゆる規模の物体に設置可能です。ジグの典型的な用途は、当然ながらワークピースが収まるジグオブジェクトの追跡ですが、3D機能を活用することで、物体の異なる表面にマーカーを貼り付け、複雑な形状のオブジェクトを追跡する手法も実現しています。

ジグは、複数の3Dマーカーをグループ化したもので構成されています。3Dマーカーは、対象物が剛体であると仮定した場合、その「フレーム」（フレームとは、位置と向きを定義するために使用されるX、Y、Zの3軸のことです）内で固定された3Dオブジェクト上の位置を特定します。 マーカーが見えるときはいつでも、それがオブジェクト上の同じ点に固定されていることがわかります。ジグのマーカーグループは、オブジェクト全体を定義します。ジグは複雑な3D形状を定義することができ、その中には見えるマーカーもあれば、隠れているマーカーもあります。見えるマーカーは、オブジェクトの位置と向きを特定するのに役立ちます。隠れていたマーカーが視界に入ると、それらは現在隠れている他のマーカーの役割を補います。

マーカーを用いたジグを定義する際、カメラに対する異なる角度からマーカーが貼られた対象物の画像を多数撮影します。カメラが撮影した各フレームは、マーカーが付いた対象物の「ビュー」となります。そして各フレームにおいて、対象物上のマーカーの位置とその相互関係（変換）を計算します。十分な数のビューを撮影した後、それらを統合して対象物の全体的な3Dモデルを作成します。これが「登録（レジストレーション）」と呼ばれるプロセスです。 しかし、光学的な要因や計算の数値的安定性に関わるいくつかの理由により、対象物周辺のビューは必ずしも完全に一致するとは限りません。実際、対象物のビューを多く取得すればするほど、登録における累積誤差は大きくなり、最終的な登録結果が役に立たなくなる可能性さえあります。ここで「バンドル調整」が重要な役割を果たします。

バンドル調整（BA）とは、複数のカメラビューを登録して形状を再構築する際に生じる累積誤差を解消するための数値最適化プロセスである。従来のBAでは、カメラの光学モデルを含め、再構築に関わるほぼすべてのパラメータが最適化の対象となる。しかし、BAのプロセスを説明する前に、最適化が必要な関連パラメータを定義しておく必要がある。リチャード・シェリスキ教授の著書 ApplicationsComputer Vision: Algorithms andApplications（Springer Press、2011年、第7章、320ページ）をぜひご参照ください。

カメラがマーカー（平らな物体）を捉えている場合、カメラの原点に対するマーカーの向きを計算することができます。次の図を参照してください：

マーカーはカメラの視野内にあり、画像平面上に投影されます。これは、3D座標と2Dピクセル座標間の変換を定式化するのに役立つ概念的な枠組みです。しかし、マーカーを含むシーンを撮影する際、3D点のパラメータは不明であり、2D画像上のどこにそれらの3D点が投影されていたかを検出することしかできません。この投影は、次の式で表すことができます：

X、Y、Zは、例えばマーカーの中心の3次元座標であり、x、yは画像上の各角の2次元ピクセル位置です。これは、パラメータの曖昧さ、つまり情報の欠落を表しています。これは、現実世界の3次元点が、カメラの中心と実際の3次元点との間の光線上のどこにでも現れる可能性があるという事実によって生じます（図中の淡いオレンジ色の点を参照）。 言い換えれば、任意のスケールを持つ物体は、カメラからの距離に応じて、画像上で任意のサイズで表示される可能性があります。 また、この式には、一般性を失うことなく、物体または逆にカメラの3次元回転（rパラメータ）と並進（tパラメータ）も含まれています。fおよびcパラメータは、カメラの光学特性をモデル化する「固有パラメータ」です（この単純な例では非常に大まかに扱っています）。

とはいえ、3次元座標と2次元座標の間には線形関係があり、この方程式のすべてのパラメータが分かっていれば、(1) 2次元のピクセル座標からマーカーの実世界における3次元位置、および (2) カメラに対するマーカーの回転 ri と並進 tx, y, z を計算することができたはずである。 2次元座標を扱う場合、式中のパラメータを単純に省略することはできないことに留意すべきである。実際、ピクセル座標を求めるには、ベクトルの最後の要素で除算を行う必要がある。すなわち、x'=λx、x =x'/λ、y'=λy、y=y'/λとなる。

2次元から3次元への対応する点が十分にあれば、上記の式を（同次）線形方程式の連立に変形し、Rとtを求め出すことができます。3Dマーカーを使用することで、各マーカーにつき少なくとも4組のこのような2次元-3次元の対応する点ペアを得ることができます。2次元の点は、画像を見てコーナーを検出することで得られます。 3D座標はマーカーの配置によって決まりますが、これも（マーカーを印刷したため）我々の制御下にあります。姿勢推定の全体的なプロセスは「Perspective-n-Point」として知られており、その解法には多くのアプローチやアルゴリズムが存在します。例えば、整列された2D-3D点群からOpenCVを用いてPythonでカメラの姿勢を求める方法は以下の通りです：

 _, 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]を求める以下の最小化問題を解くことになります：

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

再投影された3次元点と2次元点との差を「残差」と呼びます。また、一般に、残差を二乗するため、この問題を「最小二乗問題」と呼びます。Proj(.)演算子が非線形であるため、この特定のケースは非線形最小二乗問題となります。この定式化を用いることで、例えばカメラの固有パラメータを最適化問題に取り入れ、それらの最適値を求めることも可能になります：

\hat{[R|t]},\hat{\{P^\mathrm{3D}\}},\hat{K} = \mathop{\arg\min}_{[R|t],\{P^\mathrm{3D}\},K} \sum_i \Vert \mathrm{Proj}([R|t],P_i^{\mathrm{3D}},K) - P_i^{\mathrm{2D}} \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を実行して、様々な線形推定アルゴリズムから生じる残差誤差を低減します。

第1節で述べたように、ある特定のフレームでは、一部のマーカーは見えるが他のマーカーは見えない場合があり、マッピングが進むにつれて、マーカー同士の相対的な位置に関する手がかりが増えていきます。まず、最初に視認できるマーカーから始め、その3D構造を記録します。この構造は決して変化しないと仮定します。 例えば、マーカー1とマーカー2間の変換をT12と表記する。後のフレームでは、マーカー1は見えなくなったが、マーカー2は引き続き可視であり、マーカー3が新たに現れたとする。マーカー2から3への変換をT23とし、マーカー1から3への変換は両者の変換を連結してT13 = T12T23とする。

マッピング処理により、地図にさらなる誤差が生じ、これに加えて、前述したマーカーの3D姿勢を復元する際の固有の誤差も重なります。変換の連鎖によって誤差が累積し、最悪の場合、特異なケースが発生する可能性があります。この累積誤差を軽減するためにBAを適用しなければならず、そうしなければ、ジグのマッピング処理は失敗してしまいます。

最適化の一つの方法は、カメラ姿勢推定から得られる変換を固定することであり、これは前述のBAの定式化と似たものになります。ここでは、3D座標が与えられた条件下で、残差を最小化するCamPを求めています：

\hat{\mathrm{CamP}} = \arg\min_{\mathrm{CamP}} \sum_i \Vert \mathrm{Proj}(\mathrm{CamP},P_i^{\mathrm{3D}}) - P_i^{\mathrm{2D}} \Vert^2

ただし、カメラの姿勢は3D点から（2D-3D対応関係を通じて）導出されることに留意する。したがって、3D点自体の座標を最適化し、それに基づいてカメラの姿勢を再計算することが可能である。ここでは、カメラの姿勢を固定し、3D点に基づいて最小化を行い、2D再投影残差を最小化する最適な3D点を求める：

\hat{\{P^{\mathrm{3D}}\}} = \mathop{\arg\min}_{\{P^{\mathrm{3D}}\}} \sum_i \Vert \mathrm{Proj}(\mathrm{CamP},P_i^{\mathrm{3D}}) - P_i^{\mathrm{2D}} \Vert^2

この手法は、主に、物体上に位置する最適な3D点群を取得するのに役立ち、画像内の元の2D点に対する誤差を最小限に抑えることができます。 3Dマップ上の各点とマーカーIDとの関連性を維持することで、実行時にsolvePnPを用いて2D-3D対応関係を特定し、物体の姿勢を復元できるようにしています。新しいフレームが到着すると、マーカーの角の2D位置を特定し、マップ上の3D点と照合します。これにより、多数の2D-3D点を総合的に利用して物体の姿勢を推定し、誤差を平均化することができます。

3Dジグマップに対してBAを実行すると、2Dオフセットが減少することがはっきりと確認でき、オブジェクトの姿勢推定精度も大幅に向上することがわかります。

Tulip のジグ機能は、現場でのセンシング作業において幅広い活用シーンを提供します。新たな3Dジグ機能により、ハンドツールのようにさまざまな角度から見える複雑な工具の追跡など、新たな活用が可能になります。ジグマッピングとバンドル調整を活用することで、誤差を最小限に抑え、最適化された形状を持つ複雑なオブジェクトマップを作成できます。Tulip では、最適化機能が組み込まれたジグをTulip ご利用いただけます。 工具や作業台の設備、さらには資材の追跡にもご活用ください。