[论文解读] A tutorial on SE(3) transformation parameterizations and on-manifold optimization

Jose‐Luis Blanco|arXiv (Cornell University)|Jan 1, 2012

Robotics and Sensor-Based Localization参考文献 16被引用 178

一句话总结

本文提出了一套统一的教程，介绍如何使用欧拉角、旋转矩阵和四元数来表示和优化SE(3)变换，详细说明了它们之间的相互转换、位姿合成以及不确定性传播。通过提供经过验证的雅可比矩阵和MRPT库中的实用C++实现，该方法实现了机器人学和计算机视觉中的流形上优化。

ABSTRACT

An arbitrary rigid transformation in 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 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 the implementations have been thoroughly validated by means of unit testing and numerical estimation of the Jacobians.

研究动机与目标

• 统一并澄清使用三种常见旋转参数化方式（欧拉角、旋转矩阵和四元数）表示SE(3)变换的方法。
• 系统地推导并展示这些表示之间的转换，同时保持完整的位姿（平移+旋转）结构。
• 通过提供必要的数学工具和雅可比矩阵，实现机器人学和计算机视觉中准确且高效的流形上优化。
• 通过单元测试和数值估计验证所有转换和雅可比矩阵的正确性。
• 将所有算法映射到MRPT C++库中的对应函数，以实现即用型的实际应用。

提出的方法

• 推导出欧拉角、旋转矩阵（SO(3)）和四元数之间的显式转换公式，确保在整个SE(3)位姿空间中的一致性。
• 描述如何使用每种表示方法来合成两个SE(3)位姿或将一个位姿应用于三维点，同时保持几何精度。
• 分析SE(3)变换和合成过程中高斯不确定性的传播，展示在每种参数化下协方差矩阵如何变换。
• 推导出用于流形上最小二乘优化所必需的雅可比矩阵，这对状态估计和捆绑调整至关重要。
• 将所有算法集成到MRPT C++库中，并通过全面的单元测试和数值雅可比验证进行验证。
• 提出一个一致的数学框架，支持机器人软件栈中的理论分析与实际实现。

实验结果

研究问题

• RQ1 在完整的SE(3)位姿背景下，三种主要旋转表示方法——欧拉角、旋转矩阵和四元数——之间有何关系？
• RQ2 使用每种表示方法进行SE(3)位姿合成和三维点变换的正确且高效公式是什么？
• RQ3 当一个SE(3)位姿与另一个位姿组合或作用于一个点时，其不确定性（协方差）如何变化？
• RQ4 用于SE(3)中流形上优化的解析雅可比矩阵是什么？如何可靠地计算和验证它们？
• RQ5 如何将理论框架映射为MRPT库中可生产使用的C++代码？

主要发现

• 本文建立了一个完整且一致的框架，可在欧拉角、旋转矩阵和四元数之间进行转换，同时保持完整的SE(3)位姿结构。
• 提供了显式且经过验证的公式，用于在所有三种表示方式下进行SE(3)位姿合成和三维点变换。
• 通过在组合过程中基于雅可比矩阵变换协方差矩阵，准确地建模了SE(3)位姿的不确定性传播。
• 用于流形上优化的推导出的雅可比矩阵通过数值估计和单元测试得到验证，确保其在非线性优化流水线中的可靠性。
• 所有数学公式均已实现在MRPT C++库中，并具备完整的测试覆盖率，可立即用于机器人学和视觉应用。
• 对SE(3)表示的统一处理使得状态估计和SLAM系统中的鲁棒、高效且数值稳定的优化成为可能。