位姿中姿态的表示形式有很多种，比如：旋转矩阵、四元数、欧拉角、旋转向量等等。这里实现四种数学形式的相互转换功能，基于Eigen。
首先丢出Eigen的一个Demo：
testEigen.cpp(Demo)
1 #include<iostream> 2 using namespace std; 3 4 #include<D:Program FilesPCL 1.9.13rdPartyEigeneigen3/EigenCore> 5 #include<D:Program FilesPCL 1.9.13rdPartyEigeneigen3/EigenDense> 6 7 #include<D:Program FilesPCL 1.9.13rdPartyEigeneigen3/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 }
写在最后，一个T为4×4的变换矩阵，如果旋转分量是欧式正交群，那个这个T为：欧式变换；否者为：仿射变换。如果一个旋转矩阵，不是SO3，那么可以将其转为四元数，接着归一化，再转为旋转矩阵，这样结果就属于SO3。同理，如果一个四元数，最好对齐进行归一化处理，再转为其他形式。例如：构造函数Pose(const Eigen::Quaterniond& quaternion); 其实现中比其他构造函数多了归一化这一步骤；即：quaternion.normalized();
请看测试案例test3()，我们在第4或第5行进行四元数归一化，那么24行打印出来的矩阵就是单位阵。（不归一化，则不是单位阵）
1 void test3() 2 { 3 Eigen::Quaterniond q = { 0.1,0.35,0.2,0.3 }; 4 //q.normalize(); 5 Eigen::Matrix3d R = q.normalized().toRotationMatrix(); 6 7 cout << "R = " << endl; 8 cout << R << endl; 9 cout << endl; 10 11 Eigen::Matrix3d R_transpose = R.transpose(); 12 cout << "R.transpose = " << endl; 13 cout << R_transpose << endl; 14 cout << endl; 15 16 Eigen::Matrix3d R_inverse = R.inverse(); 17 18 cout << "R.inverse = " << endl; 19 cout << R_inverse << endl << endl; 20 cout << endl; 21 22 Eigen::Matrix3d ret = R * R.transpose(); 23 cout << "ret = " << endl; 24 cout << ret << endl << endl; 25 cout << endl; 26 }
现在，如果给定一个旋转矩阵（如果你是随机测试，请你参考博客开始部分公式，每个元素是有取值范围的，给出的数字不要太离谱）；如下测试案例test4。
第8行在底层有四元数归一化操作，所以你看下面效果图，R * R.tranpose() 近似为一个单位矩阵。
1 void test4() 2 { 3 Eigen::Matrix3d R; 4 R << 0.74, 0.08, 0.25, 5 0.2, 0.575, 0.05, 6 0.17, 0.19, 0.675; 7 Pose p1(R); 8 Pose p2(p1.quaternion()); // Pose中，针对从四元数构造，默认有归一化功能 9 R = p2.rotation(); 10 cout << "R = " << endl; 11 cout << R << endl; 12 cout << endl; 13 14 cout << "R.inverse = " << endl; 15 cout << R.inverse() << endl; 16 cout << endl; 17 18 cout << "R.transpose = " << endl; 19 cout << R.transpose() << endl; 20 cout << endl; 21 22 cout << "R * R.transpose() = " << endl; 23 cout << R * R.transpose() << endl; 24 cout << endl; 25 }
其中，R表示旋转，用旋转向量来描述（欧拉角有周期性和方向锁的问题，四元数有单位向量的约束，旋转矩阵冗余度太高且有各个基需要是单位正交的约束）；t表示平移，用平移向量来描述。R和t均采用无约束的向量进行描述，于是也均可以通过网络的学习来得到。因为R和t共有六个自由度，因此姿态估计又称为6D姿态估计。
注：旋转向量的方向代表旋转轴，模长代表旋转角的大小，旋转方向为逆时针。