#include <iostream>
using namespace std;
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <ctime>
#include <cmath>//欧拉角
#include <eigen3/Eigen/Geometry>
int main(int argc, char **argv)
{
Eigen::Matrix3d rotation_matrix3d=Eigen::Matrix3d::Identity();//单位阵
cout<<"rotation_matrix3d="<<rotation_matrix3d<<endl;
Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));
cout.precision(5);
//角轴表达
rotation_matrix3d=rotation_vector.toRotationMatrix();//将角轴转换为矩阵
cout<<"rotation_vector.matrix()="<<rotation_vector.matrix()<<endl;
cout<<"rotation_matrix3d="<<rotation_matrix3d<<endl;
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated=rotation_vector*v;//旋转过后的向量
cout<<"v_rotated="<<v_rotated.transpose()<<endl;
//欧拉角
Eigen::Vector3d euler_angles=rotation_matrix3d.eulerAngles(2,1,0);//ZYX顺序
cout<<"euler_angles="<<180/M_PI*euler_angles.transpose()<<endl; //角度表示,本身是弧度表示
//位姿变换
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//需要赋值为单位阵,后续好做旋转
cout<<"T(Identity)="<<T.matrix()<<endl;
T.rotate(rotation_vector);
T.pretranslate(Eigen::Vector3d(1,3,4));
cout<<"T="<<T.matrix()<<endl;
Eigen::Vector3d v_transformed=T*v;//虽然T是四维的,但有运算符重载按照三维计算
cout<<"v_transformed="<<v_transformed.transpose()<<endl;
//四元数
Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
cout<<"q(四元数,传vector)="<<q.coeffs()<<endl;//顺序[nx*sin(theta/2),ny*sin(theta/2),nz*sin(theta/2),cos(theta/2)]
//Quaterniond也可以传rotationmatrix
q=Eigen::Quaterniond(rotation_matrix3d);
cout<<"q(传matrix)="<<q.coeffs()<<endl;
v_rotated=q*v;//数学上是qvq',所以乘法有重载
cout<<"v_rotated="<<v_rotated.transpose()<<endl;
return 0;
}
Eigen集合模块学习(角轴,位姿,欧拉角,四元数)--参考SLAM十四讲3.6
最新推荐文章于 2024-06-28 16:20:27 发布