旋转矩阵转轴角

目录

1 原理介绍

2 数学公式推导

计算旋转角度 θ：

计算旋转轴 v：

特殊情况处理

3 流程

4 示例代码

        将旋转矩阵转换为轴角表示是计算机图形学、机器人学以及物理仿真中的一个基本操作。旋转矩阵是一个3x3的正交矩阵，用于描述坐标系的旋转，而轴角表示由一个单位旋转轴和一个绕该轴的旋转角度组成。

1 原理介绍

        旋转矩阵是一个3x3的正交矩阵，其行和列是正交单位向量。轴角表示由一个单位向量 v=(x,y,z) 和一个旋转角度 θ 组成。轴角表示是一种直观的旋转表示方式，特别适合描述单一旋转。

2 数学公式推导

        假设旋转矩阵为 RR，轴角表示为旋转轴 v=(x,y,z)和旋转角度 θ。可以通过以下步骤从旋转矩阵推导出轴角表示：

计算旋转角度 θ：

旋转矩阵的迹（主对角线元素之和）可以用来计算旋转角度：

根据旋转矩阵的性质，有：

因此，旋转角度可以表示为：

计算旋转轴 v：

如果 θ≠0，旋转轴可以通过下面的公式计算：

如果 θ=0，旋转轴可以任意选择，因为此时没有实际旋转。

特殊情况处理

• 当 θ=π 时，旋转轴可能不唯一，此时需要采用其他方法来计算旋转轴。

3 流程

1. 输入旋转矩阵：给定一个3x3的旋转矩阵 R。
2. 计算旋转角度 θ：根据旋转矩阵的迹计算旋转角度。
3. 计算旋转轴 v：使用旋转矩阵的元素计算旋转轴。
4. 输出轴角表示：返回旋转轴和旋转角度。

4 示例代码

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#define M_PI 3.1415926
Eigen::AngleAxisf rotationMatrixToAxisAngle(const Eigen::Matrix3f& R) {
    // 使用 Eigen 的 AngleAxis 类进行转换
    Eigen::AngleAxisf angleAxis;
    angleAxis.fromRotationMatrix(R);
    return angleAxis;
}

int main() {
    // 定义一个旋转矩阵
    Eigen::Matrix3f R;
    R = Eigen::AngleAxisf(M_PI / 4, Eigen::Vector3f::UnitX()); // 45度绕X轴旋转

    // 将旋转矩阵转换为轴角表示
    Eigen::AngleAxisf angleAxis = rotationMatrixToAxisAngle(R);

    std::cout << "Angle (radians): " << angleAxis.angle() << std::endl;
    std::cout << "Axis: " << angleAxis.axis().transpose() << std::endl;

    return 0;
}