1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
#include <iostream>
#include <ctime>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace std;
#define MATRIX_SIZE 50
int main(int argc, char** argv)
{
///Eigen 基本操作
Eigen::Matrix<float,2,3> matrix_23 ;
Eigen::Vector3d v_3d; //实质是Eigen::Matrix<double,3,1>
matrix_23 << 1,2,3,4,5,6; //赋值操作
v_3d << 3,2,1;
Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * v_3d;
cout<< result <<endl;
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Random();
cout<< "matrix_33 is :" << matrix_33 <<endl;
cout<< "matrix_33 transpose is :" << matrix_33.transpose() <<endl; //转置
cout<< "matrix_33 sum is :" << matrix_33.sum() <<endl; //各元素的和
cout<< "matrix_33 trace is :"<< matrix_33.trace() <<endl; //矩阵的迹
cout<< "matrix_33 inverse is :"<< matrix_33.inverse() <<endl; //逆
cout<< "matrix_33 determinant is ::"<< matrix_33.determinant() <<endl; //行列式
//特征值和特征向量
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver (matrix_33.transpose() * matrix_33);//实对称矩阵
cout<< "Eigen values = "<< eigen_solver.eigenvalues() << endl; //特征值
cout<< "Eigen vectors = "<< eigen_solver.eigenvectors() << endl; //特征向量
//解方程，对比求逆和矩阵分解的速度
Eigen::Matrix<double,MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
Eigen::Matrix < double ,MATRIX_SIZE,1> v_Nd;
v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
//求逆
clock_t time_stt = clock();
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse() * v_Nd;
cout<< " inverse time use is :"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" <<endl;
//QR分解
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout<< "Qr time use is :"<< 1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC << "ms" <<endl;
///Eigen 中四元数，欧拉角，旋转矩阵，旋转向量的转换
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));
cout.precision(3);
rotation_matrix = rotation_vector.toRotationMatrix(); //旋转向量可以转换为旋转矩阵
cout<<"rotation_matrix is :"<<rotation_matrix<<endl;
///AngleAxis 旋转向量进行坐标变换
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotated:"<<v_rotated.transpose()<<endl;
/// 旋转矩阵进行坐标变换
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose();
/// 欧拉角： 可以直接将旋转矩阵转换为欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); //(2,1,0)表示ZYX 的旋转顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
///四元数 可以将旋转向量转换为四元数
Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
cout<<"Quaterniond = \n"<<q.coeffs()<<endl;//coeffs 表示顺序是(x,y,z,w),w为实部，前三者为虚部。
/// 也可以将旋转矩阵赋予给四元数
q = Eigen::Quaterniond(rotation_matrix);
cout<<"Quaterniond = \n"<<q.coeffs()<<endl;
///四元数旋转一个矩阵
v_rotated = q*v; //注意数学形式为 qvq^{-1}
cout<<"(1,0,0) after rotated :"<<v_rotated.transpose()<<endl;
///使用欧氏变换
Eigen::Isometry3d T_o = Eigen::Isometry3d::Identity();
T_o.rotate (rotation_vector);
T_o.pretranslate(Eigen::Vector3d(1,3,4));
cout<< "Transform matrix = \n"<< T_o.matrix()<<endl;
Eigen::Vector3d v_transformed = T_o*v;
cout<<"v transformed :"<<v_transformed.transpose()<<endl;
///仿射变换
Eigen::Affine3d T_a = Eigen::Affine3d::Identity();
T_a.rotate (rotation_vector);
T_a.prescale(0.5);
T_a.pretranslate(Eigen::Vector3d(1,3,4));
cout<< "Transform matrix = \n"<< T_a.matrix()<<endl;
Eigen::Vector3d v_transformed1 = T_a*v;
cout<<"v transformed :"<<v_transformed1.transpose()<<endl;
///射影变换
/*
Eigen::Projective3d T_p;
T_p.rotate (rotation_vector);
T_p.prescale(0.5);
T_p.pretranslate(Eigen::Vector3d(1,3,4));
cout<< "Transform matrix = \n"<< T_p.matrix()<<endl;
Eigen::Vector3d v_transformed2 = T_p*v;
cout<<"v transformed :"<<v_transformed2.transpose()<<endl;
*/
return 0;
}
|
