#include<iostream>
#include<cmath>
#include<eigen3/Eigen/Core>
// 几何模块
#include<eigen3/Eigen/Geometry>using namespace std;int main(){// eigen/geometry模块提供了各种旋转和平移的表示// 3D旋转矩阵直接使用matrix3d或matrix3fEigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity(); // 对角线为1，其余为0// 旋转向量使用angleaxis，它底层不直接是matrix,但运算可以当做矩阵(因为重载了运算符)Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1)); // 沿Z轴旋转45度cout.precision(3); // 精度输出三位cout<<"rotation_matrix = \n"<<rotation_vector.matrix() <<endl; // 用matrix()转化为矩阵// 也可以直接赋值rotation_matrix = rotation_vector.toRotationMatrix();// 用angleaxis可以进行坐标变换Eigen::Vector3d v (1,0,0);Eigen::Vector3d v_rotated = rotation_vector * v;cout<<"(1,0,0) after rotation = "<<v_rotated.transpose() <<endl;// 或者用旋转矩阵v_rotated = rotation_matrix * v;cout<<"(1,0,0) after rotation = "<<v_rotated.transpose() <<endl;// 欧拉角：可以将旋转矩阵直接转换成欧拉角Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); // ZYX顺序，即yaw pitch roll顺序cout<<"yaw pitch roll = "<<euler_angles.transpose() <<endl;// 偶是变幻矩阵用Eigen::IsometryEigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 虽然称为3d,实质上是4*4矩阵T.rotate(rotation_vector); // 按照rotation_vector进行旋转T.pretranslate(Eigen::Vector3d(1,3,4)); // 把平移向量设成(1,3,4)cout<<"Transform matrix = \n"<<T.matrix()<<endl;cout<< T(1,2) <<endl; // 用变换矩阵进行坐标变换Eigen::Vector3d v_transformed = T*v; // 相当于 R×v+tcout<<"v transformed = "<<v_transformed.transpose()<<endl;// 对于仿射变换，使用Eigen::Affine3d和Eigen::Projective3d即可// 四元数// 可以直接把AngleAxis赋值给四元数，反之亦然Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);cout<<"quaternion = \n"<<q.coeffs() <<endl; // coeffs输出的顺序是（x,y,z,w)，w为实部，前三者为虚部// 也可以直接把旋转矩阵赋值给它q = Eigen::Quaterniond(rotation_matrix);cout<<"quaternion = \n"<<q.coeffs() <<endl;// 使用四元数旋转一个向量，使用重载的乘法即可v_rotated = q * v; // 数学上是 qvq^{-1}cout<<"(1,0,0) after rotation = "<<v_rotated.transpose() <<endl;return 0;
}