未完…
ROS接收到odometry格式消息:
nav_msgs::Odometry pos_msg
具有:
pos_msg.pose.pose.orientation.x;
pos_msg.pose.pose.position.x;
1.1、四元数转欧拉角
tf::Quaternion q;
tf::quaternionMsgToTF(odom.pose.pose.orientation, q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
1.2、欧拉角转四元数
tf::Quaternion q = tf::createQuaternionFromRPY(roll, pitch, yaw);
2.1、仿射矩阵Affine转xyz roll pitch yaw
pcl::getTranslationAndEulerAngles (Affine, x, y, z, roll, pitch, yaw);
2.2、 xyz roll pitch yaw 转 仿射矩阵
Eigen::Affine3f Affine = pcl::getTransformation(x, y, z, roll, pitch, yaw);
3.1、旋转矩阵R和四元数q之间相互转换
Eigen::Matrix3f rotation_R;
Eigen::Quaternionf rotation_q;
rotation_R = rotation_q.toRotationMatrix();
3.2、四元数转旋转矩阵
Eigen::Quaternionf quaternion(rotation_matrix);
4.1、matrix4f和Affine3f之间的相互转换
Eigen::Transform<float, 3, Eigen::Affine> affine (matrix);
Matrix4f matrix = affine.matrix();
这个好像也挺全的
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)