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

最新推荐文章于 2024-04-18 10:38:43 发布

他人是一面镜子，保持谦虚的态度

最新推荐文章于 2024-04-18 10:38:43 发布

阅读量2.2k

点赞数 3

分类专栏：视觉SLAM基础理论文章标签：算法矩阵线性代数

原文链接：https://blog.csdn.net/u011906844/article/details/121863578

版权

视觉SLAM基础理论专栏收录该内容

50 篇文章 88 订阅

订阅专栏

本文属于转发，感谢：Eigen中几种表示三维位姿的方式以及相互转换_鸿哲闲居的博客-CSDN博客_isometry3d

一、位姿的表示方式

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

2. 等距映射：Eigen::Isometry3d

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

四、内旋与外旋

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

一、位姿的表示方式

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

Eigen::Matrix<Type, 4, 4> transformation = Eigen::Matrix<Type, 4, 4>::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<Derived>& other) { *this = other; }
 
// 从四元数构造
Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }

2. 旋转矩阵 -> 四元数

// 从旋转矩阵构造
Quaternion(const MatrixBase<Derived>& other) { *this = other; }
 
Eigen::Matrix<double, 3, 3> 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<Derived>::toRotationMatrix(void) const;
 
Eigen::Quaterniond qua;
Eigen::Matrix<double, 3, 3> 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 <cmath>
 
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 <pcl/common/transforms.h>
#include <Eigen/Core>
     
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f tmp(T_utm_lidar.cast<float>());
pcl::getTranslationAndEulerAngles(tmp, x, y, z, roll, pitch, yaw);

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