[論文レビュー] A tutorial on $\mathbf{SE}(3)$ transformation parameterizations and on-manifold optimization

Jose‐Luis Blanco|arXiv (Cornell University)|Mar 29, 2021

Robotic Mechanisms and Dynamics被引用数 5

ひとこと要約

このチュートリアルは、ロボット工学およびコンピュータビジョン分野における$$\mathbf{SE}(3)$$変換パラメータ化—オイラー角、クォータニオン、および$$4\times4$$行列—を包括的かつ統一的に扱う。各パラメータ化の相互変換、ポーズ・ポイントおよびポーズ・ポーズの合成、不確実性の伝播、および多様体最適化に必要なヤコビ行列について詳細に説明する。本稿は、ロボット工学およびコンピュータビジョン分野におけるリー群に基づく最適化の厳密な基盤を確立し、検証済みの式とMRPT C++ライブラリへの統合を実現する。

ABSTRACT

An arbitrary rigid transformation in $\mathbf{SE}(3)$ can be separated into two parts, namely, a translation and a rigid rotation. This technical report reviews, under a unifying viewpoint, three common alternatives to representing the rotation part: sets of three (yaw-pitch-roll) Euler angles, orthogonal rotation matrices from $\mathbf{SO}(3)$ and quaternions. It will be described: (i) the equivalence between these representations and the formulas for transforming one to each other (in all cases considering the translational and rotational parts as a whole), (ii) how to compose poses with poses and poses with points in each representation and (iii) how the uncertainty of the poses (when modeled as Gaussian distributions) is affected by these transformations and compositions. Some brief notes are also given about the Jacobians required to implement least-squares optimization on manifolds, an very promising approach in recent engineering literature. The text reflects which MRPT C++ library functions implement each of the described algorithms. All formulas and their implementation have been thoroughly validated by means of unit testing and numerical estimation of the Jacobians

研究の動機と目的

• ロボット工学およびコンピュータビジョン分野における$$\mathbf{SE}(3)$$変換表現について、統一されたリファレンスの欠如に応える。
• オイラー角、クォータニオン、および剛体変換の行列表現の間での正確な変換式を提供する。
• 最小二乗最適化における多様体上でのヤコビ行列を体系的に導出し、検証する。
• ポーズ変換および合成を通じた不確実性の正確な伝播を可能にする。
• MRPT C++ライブラリへの統合と包括的なユニットテストを通じて、実装の支援を提供する。

提案手法

• 3D+YPR（ヨー・ピッチ・ロール）、3D+Quat（クォータニオン）、および$$4\times4$$行列表現の$$\mathbf{SE}(3)$$ポーズ間の明示的変換式を導出する。
• $$\mathbf{SE}(3)$$および$$\mathbf{SE}(2)$$における擬似指数写像および擬似対数写像を導入・形式化し、多様体最適化を可能にする。
• リー群理論を用いて、ポーズ合成、逆ポーズ、ポーズ・ポイント変換の主要な操作のヤコビ行列を導出する。
• $$\vee$$および$$\widehat{$}$$演算子を用いて、リー代数の要素およびその指数写像を表現する。
• MRPTライブラリにおける数値ヤコビアン推定およびユニットテストを通じて、すべての式を検証する。
• 即座にロボットソフトウェareで利用可能な、特定のMRPT C++関数への実装マッピングを提供する。

実験結果

リサーチクエスチョン

• RQ1 オイラー角、クォータニオン、行列の3つの主な$$\mathbf{SE}(3)$$パラメータ化を、不確実性の伝播を含めて体系的に互いに変換する方法は何か？
• RQ2 各パラメータ化におけるポーズの合成およびポイント変換の正確な数学的表現は何か？ また、不確から性はこれらの操作を通じてどのように変化するか？
• RQ3 $$\mathbf{SE}(3)$$変換のヤコビ行列はどのように導出し、多様体上での最適化に効果的に活用できるか？
• RQ4 擬似指数写像および擬似対数写像は、$$\mathbf{SE}(3)$$上での最適化を簡略化する役割を果たすが、それらはヤコビ行列の整合性をどのように保証するか？
• RQ5 理論的導出は、検証済みの正しさを備えた実際のC++コードにどのようにマッピングできるか？

主な発見

• 本稿は、ガウス分布による完全な不確実性モデリングを伴う、オイラー角、クォータニオン、および$$4\times4$$行列間の閉形式で検証済みの変換式を提供する。
• 擬似指数写像および擬似対数写像が形式的に定義され、恒等ヤコビ行列を示すことが示され、$$\mathbf{SE}(3)$$上での最適化を簡素化する。
• $$\mathbf{SE}(3)$$指数生成子のヤコビ行列が導出・検証され、多様体上での正確なガウス・ニュートン最適化を可能にする。
• SO(3)対数写像のヤコビ行列が明示的に計算され、検証され、頑健な姿勢推定を支援する。
• 左乗算および右乗算の両方に対応する、ポーズ合成$$D \boxplus \varepsilon = D \oplus e^{\varepsilon}$$のヤコビ行列が導出され、Graph-SLAMにおける効率的な最適化を可能にする。
• Graph-SLAMにおける複雑な誤差関数、例えば$$((P_2 \oplus e^{\varepsilon_2}) \ominus (P_1 \oplus e^{\varepsilon_1})) \ominus D$$のための完全なヤコビ行列セットが提供され、数値的一致性が検証されている。