#include <TransForms3d/TransForms.h>/*---------------------------------------角度弧度轉換----------------------------------------*/ /*** @description: 角度轉為弧度* @param {double} angle 角度值* @return 返回對應弧度值,一般在-3.14~3.14之間*/ double TransForms::Degrees(double angle) {return angle / 180 * M_PI; }/*** @description: 弧度轉為角度* @param {double} degrees 弧度值* @return 返回對應的角度值，一般在-180~180之間*/ double TransForms::Angle(double degrees) {return degrees / M_PI * 180; }/*---------------------------------------歐拉角部分---------------------------*/ /*** @description: 角度制歐拉角轉旋轉矩陣,此函數默認的旋轉順序是x-y-z.* @param {double} rx 繞x軸的旋轉.* @param {double} ry 繞y軸的旋轉.* @param {double} rz 繞z軸的旋轉.* @return {Matrix3d} 返回3?3的旋轉矩陣.*/ Matrix3d TransForms::EulerAngle2Mat(double rx, double ry, double rz) {rx = Degrees(rx);ry = Degrees(ry);rz = Degrees(rz);AngleAxisd rollAngle(AngleAxisd(rx, Vector3d::UnitX()));AngleAxisd pitchAngle(AngleAxisd(ry, Vector3d::UnitY()));AngleAxisd yawAngle(AngleAxisd(rz, Vector3d::UnitZ()));Matrix3d rotation_matrix;rotation_matrix = yawAngle * pitchAngle * rollAngle;return rotation_matrix; } /*** @description: 歐拉角轉旋轉矩陣* @param {Vector3d} eular 歐拉角rx,ry,rz * @return {Matrix3d} 返回3?3的旋轉矩陣.*/ Matrix3d TransForms::Euler2Mat(double rx, double ry, double rz) {AngleAxisd rollAngle(AngleAxisd(rx, Vector3d::UnitX()));AngleAxisd pitchAngle(AngleAxisd(ry, Vector3d::UnitY()));AngleAxisd yawAngle(AngleAxisd(rz, Vector3d::UnitZ()));Matrix3d rotation_matrix;rotation_matrix = yawAngle * pitchAngle * rollAngle;return rotation_matrix; }/*** @description:歐拉角轉四元數* @param {double} rx 繞x軸的旋轉* @param {double} ry 繞y軸的旋轉* @param {double} rz 繞z軸的旋轉* @return {Quaterniond} 返回對應的四元數*/ Quaterniond TransForms::Euler2Quat(double rx, double ry, double rz) {return Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ()); }/*** @description: 角度制歐拉角轉四元數* @param {double} rx 繞x軸的旋轉* @param {double} ry 繞y軸的旋轉* @param {double} rz 繞z軸的旋轉* @return {Quaterniond} 返回對應的四元數*/ Quaterniond TransForms::EulerAngle2Quat(double rx, double ry, double rz) {rx = Degrees(rx);ry = Degrees(ry);rz = Degrees(rz);return Eigen::AngleAxisd(rx, ::Eigen::Vector3d::UnitX()) *Eigen::AngleAxisd(ry, ::Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(rz, ::Eigen::Vector3d::UnitZ()); } /*** @description: 旋轉矩陣轉歐拉角(弧度制)* @param {Matrix3d} 3?3的旋轉矩陣* @return {Vector3d} 歐拉角*/ Vector3d TransForms::Mat2Euler(Matrix3d mat) {return mat.eulerAngles(2, 1, 0); }/*** @description: 歐拉角轉旋轉矩陣* @param {double} rx 繞x軸的旋轉* @param {double} ry 繞y軸的旋轉* @param {double} rz 繞z軸的旋轉* @return {*}*/ Matrix3d TransForms::EulerAngle2Mat(Vector3d eular) {return EulerAngle2Mat(eular.x(), eular.y(), eular.z()); }/*** @description: 旋轉矩陣轉角度制歐拉角(角度制)* @param {Matrix3d} 3?3的旋轉矩陣* @return {Vector3d} 歐拉角 */ Vector3d TransForms::Mat2EulerAngle(Matrix3d mat) {Vector3d rot = mat.eulerAngles(0, 1, 2);rot = rot / M_PI * 180;return rot; }/*---------------------------------------四元數部分----------------------------------------*/ /*** @description: 四元數轉旋轉矩陣* @param {Quaterniond} 四元數* @return {Matrix3d} 對應的旋轉矩陣*/ Matrix3d TransForms::Quat2Mat(Quaterniond quat) {return quat.matrix(); }/*** @description: 四元數轉歐拉角* @param {Quaterniond} 四元數* @return {Vector3d} 對應的歐拉角*/ Vector3d TransForms::Quat2Eular(Quaterniond quat) {return Mat2Euler(quat.matrix()); }/*** @description: 四元數轉弧度制歐拉角(角度制)* @param {Quaterniond} 四元數* @return {Vector3d} 對應的歐拉角*/ Vector3d TransForms::Quat2EularAngle(Quaterniond quat) {return Mat2EulerAngle(quat.matrix()); }/*** @description: 旋轉矩陣轉四元數* @param {Matrix3d} 3?3的旋轉矩陣* @return {Quaterniond} 對應的四元數*/ Quaterniond TransForms::Mat2Quat(Matrix3d mat) {return Quaterniond(mat); }/*---------------------------------------齊次矩陣部分----------------------------------------*/ /*** @description: 通過位置和歐拉角合成一個齊次矩陣* @param {Vector3d} positon 平移位置* @param {Vector3d} rotEular 旋轉變換(歐拉角形式)* @return {*}*/ Matrix4d TransForms::Compose(Vector3d positon, Vector3d rotEular) {Matrix3d rot = TransForms::EulerAngle2Mat(rotEular);// std::cout<<Mat2EulerAngle(rot);Matrix4d t;t.setIdentity();t.block<3, 3>(0, 0) = rot;t.block<3, 1>(0, 3) = positon;return t; }/*** @description: 通過位置和四元數合成一個齊次矩陣* @param {Vector3d} positon 平移位置* @param {Quaterniond} quat 四元數* @return {Matrix4d} 齊次矩陣*/ Matrix4d TransForms::Compose(Vector3d positon, Quaterniond quat) {return Compose(positon, Quat2Eular(quat)); }/*** @description: 通過三個位置和三個歐拉角合成一個齊次矩陣* @param {double} x 沿x軸的平移* @param {double} y 沿y軸的平移* @param {double} z 沿z軸的平移* @param {double} rx 繞x軸的旋轉* @param {double} ry 繞y軸的旋轉* @param {double} rz 繞z軸的旋轉* @return {Matrix4d} 返回4?4的齊次變換矩陣*/ Matrix4d TransForms::ComposeEuler(const double x, const double y, const double z, const double rx, const double ry, const double rz) {Eigen::Vector3d rot(rx, ry, rz);Eigen::Vector3d pos(x, y, z);return TransForms::Compose(pos, rot); }/*** @description: 將齊次矩陣轉換成平移和歐拉角形式，方便理解* @param {Matrix4d} 4?4的齊次變換矩陣* @return {VectorXd} x,y,z,rx,ry,rz*/ VectorXd TransForms::H2EulerAngle(Matrix4d t) {VectorXd pose = VectorXd(6);Matrix3d mt = t.block<3, 3>(0, 0);Vector3d p3 = t.block<3, 1>(0, 3).col(0);pose(0, 0) = p3.x();pose(1, 0) = p3.y();pose(2, 0) = p3.z();Vector3d eular = Mat2EulerAngle(mt);pose(3, 0) = eular.x();pose(4, 0) = eular.y();pose(5, 0) = eular.z();return pose; }

總結

以上是生活随笔為你收集整理的四元数，欧拉角，旋转矩阵相互转换的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。