參考的博客:
https://blog.csdn.net/yang__jing/article/details/82316093
https://blog.csdn.net/u011092188/article/details/77430988
Eigen庫變換
旋轉矩陣(3X3):Eigen::Matrix3d
旋轉向量(3X1):Eigen::AngleAxisd
四元數(4X1): Eigen::Quaterniond
平移向量(3X1):Eigen::Vector3d
變換矩陣(4X4):Eigen::Isometry3d
一.旋轉矩陣
1.初始賦值:
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.旋轉矩陣轉歐拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);
3.旋轉矩陣轉四元數
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;
二.歐拉角
1.初始化歐拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle(yaw,pitch,roll);
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.歐拉角轉四元數
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;
三.四元數
- 初始化四元數
Eigen::Quaterniond quaternion(w,x,y,z);
- 四元數轉旋轉矩陣
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
- 四元數轉歐拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);