Eigen中几种表示三维位姿的方式以及相互转换

目录

一、位姿的表示方式

1. 普通4*4矩阵 Eigen::Matrix

2. 等距映射：Eigen::Isometry3d

二、旋转的表示方式与相互转换

1. 四元数的几种初始化方式

2. 旋转矩阵 -> 四元数

3.  欧拉角 -> 四元数

4. 四元数 -> 旋转矩阵

5. 四元数 -> 欧拉角

6. 旋转矩阵 -> 欧拉角

7. 欧拉角 -> 旋转矩阵

三、关于“旋转矩阵、欧拉角、轴角、四元数”之间的联系与思考

四、内旋与外旋

五、Eigen::eulerAngles(2,1,0)有大坑

---

一、位姿的表示方式

1. 普通4*4矩阵 Eigen::Matrix

Eigen::Matrix transformation = Eigen::Matrix::Identity().

2. 等距映射：Eigen::Isometry3d

Eigen::Quaterniond quaternion;

// 依次旋转yaw, pitch, roll
// 即： R = R_z * R_y * R_x
quaternion = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());

Eigen::Isometry3d iso = Eigen::Translation3d(x, y, z) * quaternion;

Eigen::Matrix4d res = iso.matrix();

二、旋转的表示方式与相互转换

1. 四元数的几种初始化方式

Eigen::Quaterniond quaternion;

// 从四个系数构造
Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}

// 从旋转矩阵构造
Quaternion(const MatrixBase& other) { *this = other; }

// 从四元数构造
Quaternion(const QuaternionBase& other) { this->Base::operator=(other); }

2. 旋转矩阵 -> 四元数

// 从旋转矩阵构造
Quaternion(const MatrixBase& other) { *this = other; }

Eigen::Matrix rot;
Eigen::Quaterniond qua(rot);

3.  欧拉角 -> 四元数

Eigen::Quaterniond quaternion;

// 依次旋转yaw, pitch, roll
// 即： R = R_z * R_y * R_x
quaternion = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());

 也可以通过如下方式直接矩阵公式直接计算：

4. 四元数 -> 旋转矩阵

QuaternionBase::toRotationMatrix(void) const;

Eigen::Quaterniond qua;
Eigen::Matrix rot = qua.toRotationMatrix();

5. 四元数 -> 欧拉角

Eigen::Quaterniond qua;

// 按照yaw, pitch, roll的方式进行分解。即:
// euler(0) = yaw
// euler(1) = pitch
// euler(2) = roll

Eigen::Vector3d euler = qua.toRotationMatrix().eulerAngles(2, 1, 0);
return euler;

6. 旋转矩阵 -> 欧拉角

// 按照yaw, pitch, roll的方式进行分解。即:
// euler(0) = yaw
// euler(1) = pitch
// euler(2) = roll

Eigen::Matrix3d m;
Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);

7. 欧拉角 -> 旋转矩阵

// 先转四元数, 再转旋转矩阵


// 依次旋转yaw, pitch, roll
// 即： R = R_z * R_y * R_x
Eigen::Quaterniond quaternion = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())*
                                Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())*
                                Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());

Eigen::Matrix3d rot = quaternion.toRotationMatrix();

三、关于“旋转矩阵、欧拉角、轴角、四元数”之间的联系与思考

四、内旋与外旋

欧拉角和旋转矩阵之间的转换 - 知乎

五、Eigen::eulerAngles(2,1,0)有大坑

        原文链接：https://blog.csdn.net/delovsam/article/details/104432185

     普通的方法是，用Eigen，把四元数转成旋转矩阵，再从旋转矩阵转到欧拉角：
::Eigen::Quaterniond q(w, x, y, z);
::Eigen::Matrix3d rx = q.toRotationMatrix();
::Eigen::Vector3d ea = rx.eulerAngles(2,1,0);
但这个方法存在问题，即可能转出来的欧拉角，不是想要的，因为因为同一个四元数，可以用2个欧拉角来表示，而这个方法得到的结果有可能是用转角大于2PI的方式表达的。例如，四元数(0.00392036, -0.00511095, -0.613622, 0.789573)应当转为欧拉角(-1.32133, -0.00325971, 0.0124636)，但用Eigen却被转成了(1.82026, -3.13833, -3.12913)。

由于无人车在近似平面上运动，因此yaw角可能取值±180°，roll和pitch变化很小才对。但是使用eulerAngles(2,1,0)时，出现roll,pitch达到正负180的现象，明显错误。如下图：

为了避免这个问题，有两种解决办法：

（1）方法一： 使用 Conversion between quaternions and Euler angles（https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles） 中给出的一个算法（如下），这个算法可以保证出来的欧拉角不会超过2PI。

#define _USE_MATH_DEFINES
#include 

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

（2）方法二：使用pcl::getTranslationAndEulerAngles()。但有的文章测试认为该函数在计算绕Z轴的旋转角时存在精度损失：pcl::getTranslationAndEulerAngles精度缺失问题_Eminbogen的博客-CSDN博客

但我觉得影响不大，同时LIO-Sam中也是用的这种方式。

#include 
#include 
     
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f tmp(T_utm_lidar.cast());
pcl::getTranslationAndEulerAngles(tmp, x, y, z, roll, pitch, yaw);

        使用pcl::getTranslationAndEulerAngles()方法的效果如下：