Conversion of transformation matrix Eigen::Matrix4f to tf2 transform throws exception while broadcasting

asked 2020-07-06 05:06:44 -0500

Vaish gravatar image

updated 2020-07-06 06:52:49 -0500

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

edit retag flag offensive close merge delete