位姿转换

Posted 2020-12-13 winslam

       位姿中姿态的表示形式有很多种，比如：旋转矩阵、四元数、欧拉角、旋转向量等等。这里实现四种数学形式的相互转换功能，基于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 }

View Code

下面是姿态转换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 }