vins中旋转矩阵 四元数 转 欧拉角的方法
1、旋转矩阵或四元数 到 欧拉角的方法.
Utility::R2ypr和.eulerAngles(2,1,0)都可以，但是单位不同！
（1）Utility::R2ypr(q_array[i - j].toRotationMatrix())
输出的是：yaw pitch roll 的vector3d向量，单位是度数，（正负180）
（2）q_array[i - j].toRotationMatrix().eulerAngles(2,1,0)
输出的是：yaw pitch roll 的vector3d向量，单位是rad，（正负3.14）
cout<<"仅仅为了测试一下："<<endl;
cout<<"Utility::R2ypr(q_array[i - j].toRotationMatrix()) = "<<Utility::R2ypr(q_array[i - j].toRotationMatrix()).transpose()<<endl;
cout<<"q_array[i - j].toRotationMatrix().eulerAngles(2,1,0) = "<<q_array[i - j].toRotationMatrix().eulerAngles(2,1,0).transpose()<<endl;
结果如下：
仅仅为了测试一下：
Utility::R2ypr(q_array[i - j].toRotationMatrix()) = -0.00189955 0.00368489 89.9943
q_array[i - j].toRotationMatrix().eulerAngles(2,1,0) = 3.14156 3.14153 -1.5709
2、 欧拉角 到 旋转矩阵或四元数 的方法.
（1）Utility::ypr2R(Vector3d(-90, 0, 90)); //R_baselink_imu
输入的是：yaw pitch roll 的vector3d向量，单位是度数，（正负180）
extri_T_baselink_imu.block(0, 0) = Utility::ypr2R(Vector3d(-90, 0, 90)); //R_baselink_imu
（2）算出3轴的旋转向量/旋转矩阵，联乘后得到旋转矩阵
注意：ypr是yaw矩阵是在最左边
double yaw = PI/3,pitching = PI/4,droll = PI/6;
::Eigen::Vector3d ea0(yaw,pitching,droll);
::Eigen::Matrix3d R;
R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())
* ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())
* ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());
其他方法：不推荐，繁琐
/**
欧拉角计算对应的旋转矩阵
**/
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// 计算旋转矩阵的X分量
Mat R_x = (Mat_(3,3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);
// 计算旋转矩阵的Y分量
Mat R_y = (Mat_(3,3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);
// 计算旋转矩阵的Z分量
Mat R_z = (Mat_(3,3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1);
// 合并
Mat R = R_z * R_y * R_x;
return R;
}
以下为 转载：
一、旋转向量
1.0 初始化旋转向量：旋转角为alpha，旋转轴为(x,y,z)
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
1.1 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.toRotationMatrix();
1.2 旋转向量转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);
1.3 旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;Quaterniond quaternion;
Eigen::Quaterniond quaternion;quaternion=rotation_vector;
二、旋转矩阵
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);
Eigen::AngleAxisd rotation_vector;rotation_vector=rotation_matrix;
Eigen::AngleAxisd rotation_vector;rotation_vector.fromRotationMatrix(rotation_matrix);
2.2 旋转向量转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);
2.3 旋转向量转四元数
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;quaternion=rotation_matrix;
三、欧拉角
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;rotation_vector=yawAngle*pitchAngle*rollAngle;
3.2 欧拉角转旋转矩阵
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::Matrix3d rotation_matrix;rotation_matrix=yawAngle*pitchAngle*rollAngle;
3.3 欧拉角转四元数
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::Quaterniond quaternion;quaternion=yawAngle*pitchAngle*rollAngle;
四、四元数
4.0 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);
4.1 四元数转旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;rotation_vector=quaternion;
4.2 四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.toRotationMatrix();
4.4 四元数转欧拉角(Z-Y-X，即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
作者：delovsam
相关文章
Serwa
2020-03-20
Vicki
2021-02-11
Isoke
2021-06-16
Chloe
2020-10-28
Rae
2023-07-22
Rhoda
2023-07-22
Hester
2023-07-22
Grace
2023-07-22
Vanna
2023-07-22
Peony
2023-07-22
Dorothy
2023-07-22
Dulcea
2023-07-22
Zandra
2023-07-22
Serafina
2023-07-24
Kathy
2023-08-08
Olivia
2023-08-08
Elina
2023-08-08
Jacinthe
2023-08-08