位姿轉換
阿新 • • 發佈：2020-06-30
位姿中姿態的表示形式有很多種，比如：旋轉矩陣、四元數、尤拉角、旋轉向量等等。這裡實現四種數學形式的相互轉換功能，基於Eigen。
首先丟出Eigen的一個Demo：
testEigen.cpp(Demo)
1 #include<iostream> 2 using namespace std; 3 4 #include<D:\Program Files\PCL 1.9.1\3rdParty\Eigen\eigen3/Eigen\Core> 5 #include<D:\Program Files\PCL 1.9View Code.1\3rdParty\Eigen\eigen3/Eigen\Dense> 6 7 #include<D:\Program Files\PCL 1.9.1\3rdParty\Eigen\eigen3/Eigen/Geometry> 8 9 const double M_PI = 3.1415926535; 10 11 void Test1() 12 { 13 Eigen::Matrix<float, 2, 3> matrix_23 = Eigen::Matrix<float, 2, 3>::Ones(); 14 Eigen::Vector3d v_3d = Eigen::Vector3d::Zero();//3x1 15 Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();//3x3 16 // 動態size矩陣 17 Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic; 18 // 更簡單的表達形式 19 Eigen::MatrixXd matrix_x; 20 21 // operate 22 23 matrix_23 << 1, 2, 3, 4, 5, 6; 24 v_3d << 3, 2, 1; 25 cout << matrix_23 << endl; 26 cout << endl; 27 cout << v_3d << endl; 28 cout << endl; 29 // 乘法 float -> double 30 Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d; 31 cout << result << endl; 32 cout << endl; 33 34 matrix_33 = Eigen::Matrix3d::Random();// 偽隨機 35 cout << matrix_33 << endl; 36 cout << endl; 37 38 cout << matrix_23.transpose() << endl; cout << endl; 39 cout << matrix_23.sum() << endl; cout << endl; 40 cout << matrix_33.trace() << endl; cout << endl; 41 cout << 10 * matrix_33 << endl; cout << endl; 42 cout << matrix_33.inverse() << endl; cout << endl; 43 cout << matrix_33.determinant() << endl; cout << endl; 44 45 // 特徵值 46 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33); 47 cout << "values = " << eigen_solver.eigenvalues() << endl; cout << endl; 48 cout << "vectors = " << eigen_solver.eigenvectors() << endl; cout << endl; 49 50 // 解方程 A * x = b 51 Eigen::Matrix<double, 50, 50> A = Eigen::Matrix<double, 50, 50>::Random(); 52 Eigen::Matrix<double, 50, 1 > b = Eigen::Matrix<double, 50, 1 >::Random(); 53 54 // 如果無解， eigen 是怎麼解決這個問題的？？？？？？ 55 // 法一、x = inv(A)*b 56 cout << A.inverse() * b << endl; cout << endl; 57 58 // 法二、QR分解 59 cout << A.colPivHouseholderQr().solve(b) << endl; cout << endl; 60 } 61 62 void Test2() 63 { 64 Eigen::Matrix3d R_mat = Eigen::Matrix3d::Identity(); 65 Eigen::AngleAxisd R_vec(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 繞Z軸旋轉Π/4 66 // AngleAxisd -> Matrix3d 67 R_mat = R_vec.matrix(); 68 cout << R_vec.toRotationMatrix() << endl; cout << endl; 69 cout << R_vec.matrix() << endl; cout << endl; 70 71 // 座標旋轉：point2 = R_vec * point1 72 Eigen::Vector3d point1(1, 0, 0); 73 Eigen::Vector3d point2 = R_vec * point1; // point2 = R_mat * point1 74 cout << point2 << endl; cout << endl; 75 76 // 旋轉矩陣 -> 尤拉角 77 Eigen::Vector3d ZYX_angle = R_mat.eulerAngles(2, 1, 0);// Z Y X 順序； 78 cout << ZYX_angle << endl; cout << endl; 79 80 // 初始化一個T = (R|t) 81 Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); 82 T.rotate(R_vec); 83 T.pretranslate(Eigen::Vector3d(1, 3, 4)); 84 cout << "T = " << endl; 85 cout << T.matrix() << endl; cout << endl; 86 87 // point3 = T * point1 88 Eigen::Vector3d point3 = T * point1; 89 cout << point3 << endl; cout << endl; 90 91 // 初始化一個四元數 92 Eigen::Quaterniond q = Eigen::Quaterniond(R_vec); 93 cout << "q = " << endl; 94 cout << q.coeffs() << endl; cout << endl; 95 96 // point4 = q * point1 97 Eigen::Vector3d point4 = q * point1; 98 cout << point4 << endl; cout << endl; 99 } 100 101 /********************************************************************************************************** 102 103 功能：建立一個齊次變換矩陣 T = (R|t)； 104 105 輸入：roll(X軸)、pitch(Y軸)、yaw(Z軸)：尤拉角； 106 (x, y, z):位移； 107 108 輸出：齊次變換矩陣 T 109 110 返回：... 111 112 **********************************************************************************************************/ 113 Eigen::Isometry3d CreateT(const double& roll, const double& pitch, const double& yaw, 114 const double& x, const double& y, const double& z) 115 { 116 // <1> 初始化R 117 Eigen::Matrix3d R; 118 // 按照 ZYX 順序旋轉 119 R = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * 120 Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * 121 Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()); 122 123 // <2> 初始化t 124 Eigen::Vector3d t(x, y, z); 125 126 // <3> 構建T = (R|t) 127 Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); 128 T.rotate(R); 129 T.pretranslate(t); 130 return T; 131 } 132 133 134 135 int main() 136 { 137 // <1> 由尤拉角 + 位姿 初始化姿態; Pose1 -> 齊次變換矩陣 T1 138 Eigen::Isometry3d T1 = CreateT((30.0 / 180.0) * M_PI, (25.0 / 180.0) * M_PI, (27.0 / 180.0) * M_PI, 1.2, 0.234, 2.3); 139 Eigen::Isometry3d T2 = CreateT((23.0 / 180.0) * M_PI, (33.0 / 180.0) * M_PI, (89.0 / 180.0) * M_PI, 0.1, 0.4, 0.1); 140 141 cout << "<1> 由尤拉角 + 位姿 初始化姿態; Pose1 -> 齊次變換矩陣 T1" << endl; 142 cout << "T1 = " << endl; cout << T1.matrix() << endl; 143 cout << "T2 = " << endl; cout << T2.matrix() << endl; 144 cout << endl; 145 146 // <2> Pose1求逆（等價於T1求逆） 147 Eigen::Isometry3d T1Invert = T1.inverse(); 148 Eigen::Isometry3d T2Invert = T2.inverse(); 149 150 cout << "<2> Pose1求逆（等價於T1求逆）" << endl; 151 cout << "T1Invert = " << endl; cout << T1Invert.matrix() << endl; 152 cout << "T2Invert = " << endl; cout << T2Invert.matrix() << endl; 153 cout << endl; 154 155 // <3> 齊次變換矩陣 T1 -> 尤拉角 + 位姿 Pose1 156 cout << "<3> 齊次變換矩陣 T1 -> 尤拉角 + 位姿 Pose1" << endl; 157 cout << "Pose1 = "; 158 cout << T1.translation().transpose() << " " << T1.rotation().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) << endl; 159 cout << endl; 160 161 // <4> Pose12 = Pose1 * Pose2 等價於 T12 = T1 * T2 162 Eigen::Isometry3d T12 = T1 * T2; 163 164 cout << "<4> Pose12 = Pose1 * Pose2 等價於 T12 = T1 * T2" << endl; 165 cout << "T12 = " << endl; cout << T12.matrix() << endl; 166 cout << endl; 167 168 // <5> Pose1(旋轉部分) -> 四元數 169 Eigen::Quaterniond q = Eigen::Quaterniond(T1.rotation()); 170 171 cout << " <5> Pose1(旋轉部分) -> 四元數" << endl; 172 cout << "q = " << endl; cout << q.coeffs().transpose() << endl; // coeffs 中實部在最後 173 174 // <6> 四元數 -> Pose1__ 175 cout << " <6> 四元數 -> Pose1__(旋轉部分)" << endl; 176 cout << "Pose1__ = " << endl; cout << q.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) << endl; 177 178 return 1; 179 }
下面是姿態轉換C++程式碼：
test_geometry.cpp(測試主函式)
1 #include<iostream> 2 using namespace std; 3 4 #include"geometry.h" 5 const double M_PI = 3.1415926535; 6 7 // 對於同一個姿態，從不同的數學形式(旋轉矩陣、四元數、尤拉角、角軸)構造類Pose 8 // 依次得到 pose1 pose2 pose3 pose4 9 void testClassPose(const Eigen::Matrix3d& R1) 10 { 11 12 Pose pose1(R1); 13 cout << "旋轉矩陣 = " << endl; cout << pose1.rotation() << endl; 14 cout << "尤拉角 = " << endl; cout << pose1.euler_angle().transpose()*(180 / M_PI) << endl; 15 cout << "四元數 = " << endl; cout << pose1.quaternion().coeffs().transpose() << endl; 16 cout << "角軸 = " << endl; 17 cout << pose1.angle_axis().angle()* (180 / M_PI) <<" " << pose1.angle_axis().axis().transpose() <<endl; 18 cout << "-----------------------------" << endl; 19 20 Pose pose2(pose1.euler_angle()); 21 cout << "旋轉矩陣 = " << endl; cout << pose2.rotation() << endl; 22 cout << "尤拉角 = " << endl; cout << pose2.euler_angle().transpose()*(180 / M_PI) << endl; 23 cout << "四元數 = " << endl; cout << pose2.quaternion().coeffs().transpose() << endl; 24 cout << "角軸 = " << endl; 25 cout << pose2.angle_axis().angle()* (180 / M_PI) << " " << pose2.angle_axis().axis().transpose() << endl; 26 cout << "-----------------------------" << endl; 27 28 29 Pose pose3(pose1.angle_axis()); 30 cout << "旋轉矩陣 = " << endl; cout << pose3.rotation() << endl; 31 cout << "尤拉角 = " << endl; cout << pose3.euler_angle().transpose()*(180 / M_PI) << endl; 32 cout << "四元數 = " << endl; cout << pose3.quaternion().coeffs().transpose() << endl; 33 cout << "角軸 = " << endl; 34 cout << pose3.angle_axis().angle()* (180 / M_PI) << " " << pose3.angle_axis().axis().transpose() << endl; 35 cout << "-----------------------------" << endl; 36 37 38 Pose pose4 = pose3; 39 cout << "旋轉矩陣 = " << endl; cout << pose4.rotation() << endl; 40 cout << "尤拉角 = " << endl; cout << pose4.euler_angle().transpose()*(180 / M_PI) << endl; 41 cout << "四元數 = " << endl; cout << pose4.quaternion().coeffs().transpose() << endl; 42 cout << "角軸 = " << endl; 43 cout << pose4.angle_axis().angle()* (180 / M_PI) << " " << pose4.angle_axis().axis().transpose() << endl; 44 cout << "-----------------------------" << endl; 45 46 47 } 48 49 // 測試求逆、compose等 50 void testTheOthers(Eigen::Matrix3d R1, Eigen::Vector3d t1, 51 Eigen::Matrix3d R2, Eigen::Vector3d t2) 52 { 53 // 初始化T1 54 Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); 55 T1.prerotate(R1); T1.pretranslate(t1); 56 cout << "T1" << endl; cout << T1.matrix() << endl; 57 58 // 初始化T2 59 Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity(); 60 T2.prerotate(R2); T2.pretranslate(t2); 61 cout << "T2" << endl; cout << T2.matrix() << endl; 62 63 // 求逆 64 Eigen::Isometry3d T1_inverse = inverse(T1); 65 cout << "T1_inverse = " << endl; cout << T1_inverse.matrix() << endl; 66 67 // compose 68 Eigen::Isometry3d T12 = compose(T1, T2); 69 cout << "T12 = " << endl; cout << T12.matrix() << endl; 70 71 72 /*cout << "Rotation = " << endl; 73 cout << T1.rotation() * T2.rotation() << endl; 74 75 cout << "Translation = " << endl; 76 cout << T1.rotation() * T2.translation() + T1.translation() << endl;*/ 77 78 } 79 80 int main() 81 { 82 Eigen::Matrix3d R1; //R1 83 R1 = Eigen::AngleAxisd((30.0 / 180) * M_PI, Eigen::Vector3d::UnitX())* 84 Eigen::AngleAxisd((25.0 / 180) * M_PI, Eigen::Vector3d::UnitY())* 85 Eigen::AngleAxisd((27.0 / 180) * M_PI, Eigen::Vector3d::UnitZ()); 86 Eigen::Vector3d t1(1.2, 0.234, 2.3);//t1 87 88 Eigen::Matrix3d R2; //R2 89 R2 = Eigen::AngleAxisd((23.0 / 180) * M_PI, Eigen::Vector3d::UnitX())* 90 Eigen::AngleAxisd((33.0 / 180) * M_PI, Eigen::Vector3d::UnitY())* 91 Eigen::AngleAxisd((89.0 / 180) * M_PI, Eigen::Vector3d::UnitZ()); 92 Eigen::Vector3d t2(0.1, 0.4, 0.1); //t2 93 94 // <1> test Class Pose 95 //testClassPose(R1); 96 97 // <2> test halcon's api 98 testTheOthers(R1, t1, R2, t2); 99 100 101 return 1; 102 }
Pose.h(類Pose宣告)
1 #pragma once 2 #ifndef POSE_H 3 #define POSE_H 4 5 #include<Eigen/Core> 6 #include<Eigen/Geometry> 7 8 // namespace Geometry 9 10 class Pose 11 { 12 public: 13 Pose(); 14 15 Pose& operator= (const Pose& pose); 16 17 // construct from rotation 18 Pose(const Eigen::Matrix3d& rotation); 19 20 // construct from quaternion 21 Pose(const Eigen::Quaterniond& quaternion); 22 23 // construct from angle axisd 24 Pose(const Eigen::AngleAxisd& angle_axis); 25 26 // construct from euler angle 27 Pose(const Eigen::Vector3d& euler_angle); 28 29 ~Pose(); 30 31 // return rotation 32 Eigen::Matrix3d rotation() const; 33 34 // return quaterniond 35 Eigen::Quaterniond quaternion() const; 36 37 // return angle axisd 38 Eigen::AngleAxisd angle_axis() const; 39 40 // return euler angle 41 Eigen::Vector3d euler_angle() const; 42 43 private: 44 45 Eigen::Matrix3d rotation_; // 旋轉矩陣 46 47 Eigen::Quaterniond quaternion_; // 四元數 48 49 Eigen::AngleAxisd angle_axis_; // 角軸 50 51 Eigen::Vector3d euler_angle_; // 尤拉角 roll(X軸) pitch(Y軸) yaw(Z軸) 52 53 }; 54 55 // 姿態組合 56 Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2); 57 58 // 求逆 59 Eigen::Isometry3d inverse(const Eigen::Isometry3d& T); 60 61 62 #endif // !POSE_H
Pose.cpp(類Pose的實現)
1 #include "Pose.h" 2 3 4 Pose::Pose() 5 {} 6 7 8 Pose& Pose::operator= (const Pose& pose) 9 { 10 this->rotation_ = pose.rotation(); 11 this->quaternion_ = pose.quaternion(); 12 this->angle_axis_ = pose.angle_axis(); 13 this->euler_angle_ = pose.euler_angle(); 14 return *this; 15 } 16 17 // 18 Pose::Pose(const Eigen::Matrix3d& rotation) : 19 rotation_(rotation), 20 quaternion_(Eigen::Quaterniond(rotation_)), 21 angle_axis_(Eigen::AngleAxisd(rotation_)), 22 euler_angle_(rotation_.eulerAngles(0, 1, 2)) 23 {} 24 25 Pose::Pose(const Eigen::Quaterniond& quaternion) 26 { 27 quaternion.normalized(); 28 29 this->rotation_ = quaternion.toRotationMatrix(); 30 this->quaternion_ = Eigen::Quaterniond(rotation_); 31 this->angle_axis_ = Eigen::AngleAxisd(rotation_); 32 this->euler_angle_ = rotation_.eulerAngles(0, 1, 2); 33 } 34 35 36 Pose::Pose(const Eigen::AngleAxisd& angle_axis) : 37 rotation_(angle_axis), 38 quaternion_(Eigen::Quaterniond(rotation_)), 39 angle_axis_(Eigen::AngleAxisd(rotation_)), 40 euler_angle_(rotation_.eulerAngles(0, 1, 2)) 41 {} 42 43 44 Pose::Pose(const Eigen::Vector3d& euler_angle) : 45 rotation_(Eigen::AngleAxisd(euler_angle.x(), Eigen::Vector3d::UnitX()) * // note: ZYX 46 Eigen::AngleAxisd(euler_angle.y(), Eigen::Vector3d::UnitY()) * 47 Eigen::AngleAxisd(euler_angle.z(), Eigen::Vector3d::UnitZ())), 48 quaternion_(Eigen::Quaterniond(rotation_)), 49 angle_axis_(Eigen::AngleAxisd(rotation_)), 50 euler_angle_(rotation_.eulerAngles(0, 1, 2)) 51 {} 52 53 54 Pose::~Pose() 55 {} 56 57 58 Eigen::Matrix3d Pose::rotation() const 59 { 60 return this->rotation_; 61 } 62 63 64 Eigen::Quaterniond Pose::quaternion() const 65 { 66 return this->quaternion_; 67 } 68 69 70 Eigen::AngleAxisd Pose::angle_axis() const 71 { 72 return this->angle_axis_; 73 } 74 75 76 Eigen::Vector3d Pose::euler_angle() const 77 { 78 return this->euler_angle_; 79 } 80 81 82 Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2) 83 { 84 return T1 * T2; 85 } 86 87 Eigen::Isometry3d inverse(const Eigen::Isometry3d& T) 88 { 89 return T.inverse(); 90 }