Eigen四元数、欧拉角、旋转矩阵、旋转向量互相转换

给算法爸爸上香

已于 2022-11-12 13:42:21 修改

阅读量463

点赞数

分类专栏： Eigen 文章标签： c++ Eigen

于 2022-11-12 13:34:16 首次发布

本文链接：https://blog.csdn.net/taifyang/article/details/127819326

版权

Eigen 专栏收录该内容

2 篇文章 1 订阅

订阅专栏

1 旋转向量
1.0 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
1.1 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix=rotation_vector.toRotationMatrix();
1.2 旋转向量转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);
1.3 旋转向量转四元数
Eigen::Quaterniond quaternion=rotation_vector;

2 旋转矩阵
2.0 初始化旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
2.1 旋转矩阵转旋转向量
Eigen::AngleAxisd rotation_vector=rotation_matrix;
2.2 旋转矩阵转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);
2.3 旋转矩阵转四元数
Eigen::Quaterniond quaternion=rotation_matrix;

3 欧拉角
3.0 初始化欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle(yaw,pitch,roll);
3.1 欧拉角转旋转向量
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); 
Eigen::AngleAxisd rotation_vector=yawAngle*pitchAngle*rollAngle;
3.2 欧拉角转旋转矩阵
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ())); 
Eigen::Matrix3d rotation_matrix=yawAngle*pitchAngle*rollAngle;
3.3 欧拉角转四元数
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ())); 
Eigen::Quaterniond quaternion=yawAngle*pitchAngle*rollAngle;

4 四元数
4.0 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);
4.1 四元数转旋转向量
Eigen::AngleAxisd rotation_vector=quaternion;
4.2 四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix=quaternion.toRotationMatrix();
4.3 四元数转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);