ROS中四元数、欧拉角、旋转矩阵等格式转换

未完…
ROS接收到odometry格式消息：

nav_msgs::Odometry pos_msg

具有：

pos_msg.pose.pose.orientation.x; // xyzw
pos_msg.pose.pose.position.x; //xyz

1.1、四元数转欧拉角

tf::Quaternion q; 
tf::quaternionMsgToTF(odom.pose.pose.orientation, q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw); // rpy得是double

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();