Conversion of transformation matrix Eigen::Matrix4f to tf2 transform throws exception while broadcasting
I am trying to convert a 4x4 transformation matrix (Eigen::Matrix4f) that is an output of PCL's ICP into a tf2 Transform that I can publish. The 4x4 consists of a 3x3 rotation matrix and a 3x1 translation matrix which can be extracted. Although I do not get any errors while building the package, when I try to open the tf2 tree I see "No tf data received" or when I run
rosrun tf tf_echo world_frame transformed_frame
I get the following exception:
Failure at 1594027818.884973502
Exception thrown:"world_frame" passed to lookupTransform argument target_frame does not exist.
The current list of frames is:
Below is the relevant piece of code for conversion and broadcasting. However this is in the call back function of the node and not in main.
void
cloud_cb(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
static tf2_ros::TransformBroadcaster tf_broadcaster;
geometry_msgs::TransformStamped transform_stamped;
.
// Eigen::Matrix4f transformation;
// code to obtain icp transform of the type Eigen::Matrix4f
.
.
tf2::Matrix3x3 rotation_mat(transformation(0, 0), transformation(0, 1), transformation(0, 2), transformation(1, 0), transformation(1, 1), transformation(1, 2), transformation(2, 0), transformation(2, 1), transformation(2, 2));
tf2::Vector3 translation_mat(transformation(0, 3), transformation(1, 3), transformation(2, 3));
tf2::Transform trans(rotation_mat, translation_mat);
transform_stamped.header.stamp = ros::Time::now();
transform_stamped.header.frame_id = "world_frame";
transform_stamped.child_frame_id = "transformed_frame";
transform_stamped.transform = tf2::toMsg(trans);
tf_broadcaster.sendTransform(transform_stamped);
}
The constructor for the node and the main as below
PoseEstimation()
{
cloud_topic_ = "input";
sub_ = nh_.subscribe(cloud_topic_, 1, &PoseEstimation::cloud_cb, this);
ROS_INFO("Listening for incoming data on topic %s",
nh_.resolveName(cloud_topic_).c_str());
}
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_estimation", ros::init_options::AnonymousName);
PoseEstimation b;
ros::spin();
return (0);
}
Can somebody please suggest a solution this
Asked by Vaish on 2020-07-06 05:06:44 UTC
Comments