基于eigen实现欧拉⾓（RPY），旋转矩阵，旋转向量，四元数之
间的变换
examda在机器⼈学中经常会涉及到欧拉⾓，旋转矩阵，旋转向量，四元数之间的转换，因此基于eigen对变换关系进⾏实现，以便参考:
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
#define PI (3.1415926535897932346f)
int main(int argc, char **argv)
披头士乐队经典歌曲
{
/**** 1. 旋转向量 ****/
cout << endl << "********** AngleAxis **********" << endl;
//1.0 初始化旋转向量，沿Z轴旋转45度的旋转向量
Eigen::AngleAxisd rotation_vector1 (M_PI/4, Eigen::Vector3d(0, 0, 1));
//1.1 旋转向量转换为旋转矩阵
//旋转向量⽤matrix()转换成旋转矩阵
Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity();
rotation_matrix1 = rotation_vector1.matrix();
cout << "rotation matrix1 =\n" << rotation_matrix1 << endl;
//或者由罗德⾥格公式进⾏转换
rotation_matrix1 = RotationMatrix();
cout << "rotation matrix1 =\n" << rotation_matrix1 << endl;
/
粤语在线翻译*1.2 旋转向量转换为欧拉⾓*/
//将旋转向量转换为旋转矩阵，再由旋转矩阵转换为欧拉⾓，详见旋转矩阵转换为欧拉⾓
Eigen::Vector3d eulerAngle1 = rotation_vector1.matrix().eulerAngles(2,1,0);
cout << "eulerAngle1, z y x: " << eulerAngle1 << endl;
/*1.3 旋转向量转四元数*/
Eigen::Quaterniond quaternion1(rotation_vector1);
//或者
Eigen::Quaterniond quaternion1_1;
quaternion1_1 = rotation_vector1;
cout << "quaternion1 x: " << quaternion1.x() << endl;
cout << "quaternion1 y: " << quaternion1.y() << endl;
cout << "quaternion1 z: " << quaternion1.z() << endl;
cout << "quaternion1 w: " << quaternion1.w() << endl;
cout << "quaternion1_1 x: " << quaternion1_1.x() << endl;
cout << "quaternion1_1 y: " << quaternion1_1.y() << endl;
cout << "quaternion1_1 z: " << quaternion1_1.z() << endl;
number是什么意思
cout << "quaternion1_1 w: " << quaternion1_1.w() << endl;
/**** 2. 旋转矩阵 *****/
cout << endl << "********** RotationMatrix **********" << endl;
//2.0 旋转矩阵初始化
Eigen::Matrix3d rotation_matrix2;
rotation_matrix2 << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1;
;
halloween是什么意思//或直接单位矩阵初始化
Eigen::Matrix3d rotation_matrix2_1 = Eigen::Matrix3d::Identity();
//2.1 旋转矩阵转换为欧拉⾓
//ZYX顺序，即先绕x轴roll，再绕y轴pitch，最后绕z轴yaw,0表⽰X轴,1表⽰Y轴,2表⽰Z轴
Eigen::Vector3d euler_angles = rotation_matrix2.eulerAngles(2, 1, 0);
Eigen::Vector3d euler_angles = rotation_matrix2.eulerAngles(2, 1, 0);
cout << "yaw(z) pitch(y) roll(x) = " << anspo() << endl;
//2.2 旋转矩阵转换为旋转向量
钱用英语怎么说Eigen::AngleAxisd rotation_vector2;
rotation_vector2.fromRotationMatrix(rotation_matrix2);
//或者
Eigen::AngleAxisd rotation_vector2_1(rotation_matrix2);
cout << "rotation_vector2 " << "angle is: " << rotation_vector2.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector2.axis().transpo() << endl;
cout << "rotation_vector2_1 " << "angle is: " << rotation_vector2_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector2_1.axis().transpo() << endl;
//2.3 旋转矩阵转换为四元数
Eigen::Quaterniond quaternion2(rotation_matrix2);
//或者
感恩 演讲稿Eigen::Quaterniond quaternion2_1;
quaternion2_1 = rotation_matrix2;
cout << "quaternion2 x: " << quaternion2.x() << endl;
cout << "quaternion2 y: " << quaternion2.y() << endl;
cout << "quaternion2 z: " << quaternion2.z() << endl;
cout << "quaternion2 w: " << quaternion2.w() << endl;
cout << "quaternion2_1 x: " << quaternion2_1.x() << endl;
马修波莫cout << "quaternion2_1 y: " << quaternion2_1.y() << endl;
cout << "quaternion2_1 z: " << quaternion2_1.z() << endl;
cout << "quaternion2_1 w: " << quaternion2_1.w() << endl;
/**** 3. 欧拉⾓ ****/
cout << endl << "********** EulerAngle **********" << endl;
//3.0 初始化欧拉⾓(Z-Y-X，即RPY, 先绕x轴roll，再绕y轴pitch，最后绕z轴yaw)
Eigen::Vector3d ea(0.785398, -0, 0);
//3.1 欧拉⾓转换为旋转矩阵
Eigen::Matrix3d rotation_matrix3;
rotation_matrix3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());
cout << "rotation matrix3 =\n" << rotation_matrix3 << endl;
//3.2 欧拉⾓转换为四元数,
Eigen::Quaterniond quaternion3;
quaternion3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());
cout << "quaternion3 x: " << quaternion3.x() << endl;
cout << "quaternion3 y: " << quaternion3.y() << endl;
cout << "quaternion3 z: " << quaternion3.z() << endl;
cout << "quaternion3 w: " << quaternion3.w() << endl;
//3.3 欧拉⾓转换为旋转向量
Eigen::AngleAxisd rotation_vector3;
despairingrotation_vector3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());
cout << "rotation_vector3 " << "angle is: " << rotation_vector3.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector3.axis().transpo() << endl;
/**** 4.四元数 ****/
cout << endl << "********** Quaternion **********" << endl;
//4.0 初始化四元素，注意eigen Quaterniond类四元数初始化参数顺序为w,x,y,z
Eigen::Quaterniond quaternion4(0.92388, 0, 0, 0.382683);
//4.1 四元数转换为旋转向量
Eigen::AngleAxisd rotation_vector4(quaternion4);
//或者
Eigen::AngleAxisd rotation_vector4_1;
rotation_vector4_1 = quaternion4;
cout << "rotation_vector4 " << "angle is: " << rotation_vector4.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector4.axis().transpo() << endl;
cout << "rotation_vector4_1 " << "angle is: " << rotation_vector4_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector4_1.axis().transpo() << endl;
//4.2 四元数转换为旋转矩阵
Eigen::Matrix3d rotation_matrix4;
rotation_matrix4 = quaternion4.matrix();
Eigen::Matrix3d rotation_matrix4_1;
rotation_matrix4_1 = RotationMatrix();
cout << "rotation matrix4 =\n" << rotation_matrix4 << endl;
cout << "rotation matrix4_1 =\n" << rotation_matrix4_1 << endl;
//4.4 四元数转欧拉⾓(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle4 = quaternion4.matrix().eulerAngles(2,1,0);
cout << "yaw(z) pitch(y) roll(x) = " << anspo() << endl;
return 0;
}prentation是什么意思
终端输出:
