【tf的使用】【Eigen和tf中位姿表达方式转换】【tf::StampedTransform】【Eigen::Matrix4f】
0 前言
1 tf::StampedTransform<——>Eigen::Matrix4f
tf::StampedTransform
:是ros里代表TF变换的数据类型,一般在机器人上读取雷达坐标系到机器人坐标系的信息就是通过读取tf信息.Eigen::Matrix4f
:是变换矩阵的位姿格式,4*4的矩阵,包括平移和旋转.
1.1 实现代码
bool TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix);
bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {
Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
double roll, pitch, yaw;
tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());
transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
return true;
}
2 tf::Quaternion和geometry_msgs/Quaternion的变换,变出roll, pitch, yaw
const sensor_msgs::Imu::ConstPtr& imuIn
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
N 参考