Eigen中⼏种表⽰三维位姿的⽅式以及相互转换⽬录
⼀、位姿的表⽰⽅式
1. 普通4*4矩阵 Eigen::Matrix
Eigen::Matrix<Type, 4, 4> transformation = Eigen::Matrix<Type, 4, 4>::Identity().
ebit2. 等距映射：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 MatrixBa<Derived>& other) { *this = other; }
// 从四元数构造
Quaternion(const QuaternionBa<Derived>& other) { this->Ba::operator=(other); }
2. 旋转矩阵 -> 四元数
// 从旋转矩阵构造
Quaternion(const MatrixBa<Derived>& other) { *this = other; }
addicts
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. 四元数 -> 旋转矩阵
QuaternionBa<Derived>::toRotationMatrix(void) const;
Eigen::Quaterniond qua;
Eigen::Matrix<double, 3, 3> rot = RotationMatrix();
5. 四元数 -> 欧拉⾓
Eigen::Quaterniond qua;
// 按照yaw, pitch, roll的⽅式进⾏分解。即:
// euler(0) = yaw
// euler(1) = pitch
revi什么意思// euler(2) = roll
Eigen::Vector3d euler = RotationMatrix().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
opp// 即： 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 = RotationMatrix();
三、关于“旋转矩阵、欧拉⾓、轴⾓、四元数”之间的联系与思考
四、内旋与外旋
blackbirds五、Eigen::eulerAngles(2,1,0)有⼤坑
普通的⽅法是，⽤Eigen，把四元数转成旋转矩阵，再从旋转矩阵转到欧拉⾓：
::Eigen::Quaterniond q(w, x, y, z);
::Eigen::Matrix3d rx = q.toRotationMatrix();gay是什么意思
::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)时，出现
amplificationroll,pitch达到正负180的现象，明显错误。如下图：
为了避免这个问题，有两种解决办法：
#define _USE_MATH_DEFINES
#include <cmath>
struct Quaternion {
double w, x, y, z;
};
struct EulerAngles {
double roll, pitch, yaw;
};
EulerAngles ToEulerAngles(Quaternion q) {
paryEulerAngles 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);
// 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); // u 90 degrees if out of range
el
angles.pitch = std::asin(sinp);
// yaw (z-axis rotation)
double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
labordaydouble 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轴的旋转⾓时存在精度损失：
但我觉得影响不⼤，同时LIO-Sam中也是⽤的这种⽅式。
