[论文解读] 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)$$ 变换表示缺乏统一参考的问题。
• 提供欧拉角、四元数与刚体变换矩阵表示之间精确的转换公式。
• 系统推导并验证最小二乘法在 $$\mathbf{SE}(3)$$ 流形上优化所需的雅可比矩阵。
• 实现通过位姿变换与复合操作的精确不确定性传播。
• 通过与 MRPT C++ 库集成及广泛的单元测试，支持实际代码实现。

提出的方法

• 推导出 3D+YPR（偏航-俯仰-滚转）、3D+Quat（四元数）与 $$4\times4$$ 矩阵表示的 $$\mathbf{SE}(3)$$ 位姿之间显式转换公式。
• 引入并形式化了 $$\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}$$ 的雅可比矩阵被推导，涵盖左乘与右乘两种情形，支持 Graph-SLAM 中的高效优化。
• 本文提供了 Graph-SLAM 中复杂误差函数（如 $$((P_2 \oplus e^{\varepsilon_2}) \ominus (P_1 \oplus e^{\varepsilon_1})) \ominus D$$）的完整雅可比矩阵集合，并通过数值一致性验证。