平台与工具
- win11
- visual studio 2022 community
代码
#include<Eigen/Dense>
#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<math.h>
using namespace std;
using namespace Eigen;
#define PI acos(-1)
int main()
{
Matrix3d rotation_matrix = Matrix3d::Identity();
AngleAxisd rotation_vector(PI / 4, Vector3d(0, 0, 1));
cout.precision(3);
cout << "rotation matrix = \n" <<
rotation_vector.matrix() << endl;
rotation_matrix = rotation_vector.toRotationMatrix();
Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;
Isometry3d T = Isometry3d::Identity();
T.rotate(rotation_vector);
T.pretranslate(Vector3d(1, 3, 4));
cout << "Transform matrix = \n" << T.matrix() << endl;
Vector3d v_transformed = T * v;
cout << "v transformed = \n" << v_transformed.transpose() << endl;
Quaterniond q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
v_rotated = q * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
cout << "should be equal to " <<
(q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;
return 0;
}
结果
rotation matrix =
0.707 -0.707 0
0.707 0.707 0
0 0 1
(1,0,0) after rotation (by angle axis) = 0.707 0.707 0
(1,0,0) after rotation (by matrix) = 0.707 0.707 0
yaw pitch roll = 0.785 -0 0
Transform matrix =
0.707 -0.707 0 1
0.707 0.707 0 3
0 0 1 4
0 0 0 1
v transformed =
1.71 3.71 4
quaternion from rotation vector = 0 0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707 0
should be equal to 0.707 0.707 0 0
E:\projectFiles\CPPcodes\slam\chap3\eigen002\x64\Release\eigen002.exe (进程 35728)已退出,代码为 0。
按任意键关闭此窗口. . .