锐客网

  1. 首页 > it技术 > >

Eigen集合模块学习（角轴，位姿，欧拉角，四元数）--参考SLAM十四讲3.6

【Eigen集合模块学习（角轴，位姿，欧拉角，四元数）--参考SLAM十四讲3.6】#include
using namespace std;
#include
#include
#include
#include //欧拉角
#include
int main(int argc, char **argv)
{
Eigen::Matrix3d rotation_matrix3d=Eigen::Matrix3d::Identity(); //单位阵
cout<<"rotation_matrix3d="<Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1));
cout.precision(5);
//角轴表达
rotation_matrix3d=rotation_vector.toRotationMatrix(); //将角轴转换为矩阵
cout<<"rotation_vector.matrix()="<cout<<"rotation_matrix3d="<Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated=rotation_vector*v; //旋转过后的向量
cout<<"v_rotated="<//欧拉角
Eigen::Vector3d euler_angles=rotation_matrix3d.eulerAngles(2,1,0); //ZYX顺序
cout<<"euler_angles="<<180/M_PI*euler_angles.transpose()<//位姿变换
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); //需要赋值为单位阵，后续好做旋转
cout<<"T(Identity)="<T.rotate(rotation_vector);
T.pretranslate(Eigen::Vector3d(1,3,4));
cout<<"T="<
Eigen::Vector3d v_transformed=T*v; //虽然T是四维的，但有运算符重载按照三维计算
cout<<"v_transformed="<//四元数
Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
cout<<"q(四元数,传vector)="<//Quaterniond也可以传rotationmatrix
q=Eigen::Quaterniond(rotation_matrix3d);
cout<<"q(传matrix)="<v_rotated=q*v; //数学上是qvq',所以乘法有重载
cout<<"v_rotated="<return 0;
}

    推荐阅读


    上一篇：i2c信号的ACK与NACK

    下一篇：获取oracle数据库的表名、字段名等系统信息