机器人SLAM 中的位姿变化关系与常用消息记录
admin
2024-05-09 22:34:42
0次

目录

    • Eigen
      • 1. 旋转向量
      • 2. 旋转矩阵
      • 3. 欧拉角
      • 4. 四元数
      • 5. Isometry3d
      • 6. Eigen::Affine3f和Eigen::Matrix4f的转换
      • 7. float 和 double类型转换
    • nav_msgs::Odometry

Eigen

1. 旋转向量

1.1 初始化
旋转角: alphaalphaalpha (顺时针)，旋转轴: (x,y,z)(x,y,z)(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))

Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());

1.2 旋转向量 <-> 旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

1.3 旋转向量 -> 欧拉角

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);

1.4 旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

2. 旋转矩阵

2.1 初始化

Eigen::Matrix3d rotation_matrix;
rotation_matrix<

2.2 旋转矩阵 -> 旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);

Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;

Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

2.3 旋转矩阵 -> 欧拉角

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);

2.4 旋转矩阵 -> 四元数

Eigen::Quaterniond quaternion(rotation_matrix);

Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

3. 欧拉角

3.1 初始化

Eigen::Vector3d eulerAngle(roll,pitch,yaw);

3.2 欧拉角 -> 旋转向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

3.3 欧拉角 -> 旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

3.4 欧拉角 -> 四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

4. 四元数

4.1 初始化

Eigen::Quaterniond quaternion(w,x,y,z);

4.2 四元数 -> 旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

4.3 四元数 -> 旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

4.4 四元数 -> 欧拉角

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);

5. Isometry3d

5.1 初始化

  • 对各个元素赋值
Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1(0,0) = 1.000000e+00, T1(0,1) = 1.197624e-11, T1(0,2) = 1.704639e-10, T1(0,3) = 3.214096e-14;
T1(1,0) = 1.197625e-11, T1(1,1) = 1.197625e-11, T1(1,2) = 3.562503e-10, T1(1,3) = -1.998401e-15;
T1(2,0) = 1.704639e-10, T1(2,1) = 3.562503e-10, T1(2,2) = 1.000000e+00, T1(2,3) = -4.041212e-14;
T1(3,0) =            0, T1(3,1) =            0, T1(3,2) =            0, T1(3,3) =             1;
  • 通过旋转矩阵和平移向量
Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();
Tc1w.rotate(rotation_matrix); // 按照rotation_matrix进行旋转
Tc1w.pretranslate(t);   // 把平移向量设成t
  • Eigen::Matrix4d构造变换矩阵
Eigen::Matrix4d T2;
T2.setIdentity();
T2.block<3,3>(0,0) = rotation_matrix1;
T2.topRightCorner(3, 1) = t1;

5.2 获取旋转矩阵

Eigen::Matrix3d rotation = Tc1w.rotation();

5.2 获取平移向量

Eigen::Vector3d position = Tc1w.translation();

6. Eigen::Affine3f和Eigen::Matrix4f的转换

Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;

7. float 和 double类型转换

Eigen::MatrixXd matrix_d；
Eigen::MatrixXf matrix_f；
matrix_f = matrix_d.cast();

nav_msgs::Odometry

Header headeruint32 seqtime stampstring frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance posegeometry_msgs/Point positionfloat64 xfloat64 yfloat64 zgeometry_msgs/Quaternion orientationfloat64 xfloat64 yfloat64 zfloat64 w
geometry_msgs/TwistWithCovariance twistgeometry_msgs/Twist twistgeometry_msgs/Vector3 linearfloat64 xfloat64 yfloat64 zgeometry_msgs/Vector3 angularfloat64 xfloat64 yfloat64 zfloat64[36] covariance

持续完善…

参考文章：
Eigen位姿表示
使用Eigen实现四元数、欧拉角、旋转矩阵、旋转向量之间的转换

词库加载错误:未能找到文件“E:\highferrum_mysql\Configuration\Dict_Stopwords.txt”。

上一篇：哪里找得到一块一分的火爆跑得快红中2024@更新完成《天猫》

下一篇：「一分钟介绍』一元一分麻将微信群@哔哩哔哩

相关内容

热门资讯

火焰纹章英雄人物推荐(火焰纹章...
打发时间一元一分红中麻将跑得快... 认准微——as099055或as022055——客服扣675434346免押%D%A
《风帆纪元》紫船大全 紫船与变...
（盘点十款）1元1分正规麻将群... 正规广东红中癞子麻将，15张跑得快，一元一分群，24小时不熄火
哪里有熊猫一元一分广东麻将上下... ＋薇:mj08522或hz05832游戏类型：单挑，多人，亲友圈模式、秒上下，所有用户都是微信实名制...
《暗黑破坏神4》狼人德详细开荒...
《暗黑破坏神4》世界4冰法越1...
《塞尔达传说王国之泪》最后一个...
“发慌！””清友会管清友“是陷... 网恋有风险，恋爱需谨慎！“他们”通过网页，社交软件，等形式发布有色广告，诱骗点击链接下载APP，随即...
《崩坏星穹铁道》朋克洛德精神任...