[논문 리뷰] A tutorial on $\mathbf{SE}(3)$ transformation parameterizations and on-manifold optimization

Jose‐Luis Blanco|arXiv (Cornell University)|2021. 03. 29.

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}(2)$$에 대한 의사 지수함수 및 의사 로그함수 사상의 도입과 체계적 정의를 통해 다양체 최적화를 가능하게 한다.
• 리 군 이론을 활용하여 자세 조합, 역자세, 자세-점 변환과 같은 주요 연산에 대한 야코비안을 유도한다.
• $$\vee$$ 및 $$\widehat{$}$$ 연산자를 사용하여 리 대수 원소와 그 지수함수를 표현한다.
• 모든 공식을 수치적 야코비안 추정 및 MRPT 라이브러리 내 단위 테스트를 통해 검증한다.
• 즉각적인 로봇 소프트웨어 적용을 위해 특정 MRPT C++ 함수에 대한 구현 매핑을 제공한다.

실험 결과

연구 질문

• RQ1 오일러 각도, 퀄터니언, 행렬 표현 방식으로 나타낸 $$\mathbf{SE}(3)$$ 자세의 세 가지 주요 표현 방식을 상호 간에 체계적으로 변환하고, 불확실성 전파를 포함하여 수행할 수 있는가?
• RQ2 각 표현 방식에서 자세 조합 및 점 변환을 위한 정확한 수학적 표현은 무엇이며, 이러한 연산을 거쳐 불확실성이 어떻게 전파되는가?
• RQ3 $$\mathbf{SE}(3)$$ 변환의 야코비안은 어떻게 유도하고 다양체 최적화에 효과적으로 활용할 수 있는가?
• RQ4 의사 지수함수 및 의사 로그함수 사상은 $$\mathbf{SE}(3)$$ 상에서 최적화를 단순화하는 데 어떤 역할을 하는가? 그리고 어떻게 일관된 야코비안 행동을 보장하는가?
• RQ5 이론적 유도 결과는 MRPT 라이브러리 내 실질적인 C++ 코드로 어떻게 매핑되며, 그 정확성이 검증되었는가?

주요 결과

• 논문은 오일러 각도, 퀄터니언, $$4\times4$$ 행렬 간의 폐쇄형, 검증된 공식을 제공하며, 가우시안 분포를 통한 완전한 불확실성 모델링을 포함한다.
• 의사 지수함수 및 의사 로그함수 사상은 철저히 정의되었으며, 항등 야코비안을 유도함으로써 $$\mathbf{SE}(3)$$ 상의 최적화를 단순화한다.
• $$\mathbf{SE}(3)$$ 지수 생성자 야코비안은 유도되고 검증되었으며, 다양체 상에서 정확한 가우스-뉴턴 최적화를 가능하게 한다.
• SO(3) 로그함수 사상의 야코비안은 명시적으로 계산되고 검증되었으며, 견고한 자세 추정을 지원한다.
• 좌우 곱셈 모두를 고려한 자세 조합 $$D \boxplus \varepsilon = D \oplus e^{\varepsilon}$$ 의 야코비안은 유도되었으며, 그래프-SLAM에서의 효율적 최적화를 가능하게 한다.
• 그래프-SLAM에서 복잡한 오차 함수에 대한 완전한 야코비안 세트를 제공하였으며, 예를 들어 $$((P_2 \oplus e^{\varepsilon_2}) \ominus (P_1 \oplus e^{\varepsilon_1})) \ominus D$$ 와 같은 표현에 대해 수치적 일致성을 검증하였다.