四元数，欧拉角，旋转矩阵相互转换

最新推荐文章于 2025-01-03 14:37:44 发布

原创

最新推荐文章于 2025-01-03 14:37:44 发布 · 1k 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#slam

本文档详细介绍了角度与弧度之间的转换函数，以及如何使用欧拉角、四元数和齐次矩阵进行3D旋转矩阵的计算，包括从欧拉角到矩阵、矩阵到欧拉角、四元数的相互转换，以及齐次矩阵的合成与解析方法。

#include <TransForms3d/TransForms.h>

/*---------------------------------------角度弧度转换----------------------------------------*/
/**
     * @description: 角度转为弧度
     * @param {double} angle 角度值
     * @return 返回对应弧度值,一般在-3.14~3.14之间
     */
double TransForms::Degrees(double angle)
{
    return angle / 180 * M_PI;
}

/**
     * @description: 弧度转为角度
     * @param {double} degrees 弧度值
     * @return 返回对应的角度值，一般在-180~180之间
     */
double TransForms::Angle(double degrees)
{
    return degrees / M_PI * 180;
}

/*---------------------------------------欧拉角部分---------------------------*/
/**
     * @description: 角度制欧拉角转旋转矩阵,此函数默认的旋转顺序是x-y-z.
     * @param {double} rx 绕x轴的旋转.
     * @param {double} ry 绕y轴的旋转.
     * @param {double} rz 绕z轴的旋转.
     * @return {Matrix3d}  返回3✖3的旋转矩阵.
     */
Matrix3d TransForms::EulerAngle2Mat(double rx, double ry, double rz)
{
    rx = Degrees(rx);
    ry = Degrees(ry);
    rz = Degrees(rz);
    AngleAxisd rollAngle(AngleAxisd(rx, Vector3d::UnitX()