Ask Your Question
4

4x4 Transformation Matrix to tf Transform

asked 2013-11-20 16:56:05 -0500

TFinleyosu gravatar image

updated 2014-01-28 17:18:36 -0500

ngrennan gravatar image

I am trying to convert a 4x4 transformation matrix (Eigen::Matrix4f) that is an output of PCL's ICP into a tf Transform that I can publish. The 4x4 consists of a 3x3 rotation matrix and a 3x1 translation matrix.

Is there an easy way to make this conversion?

The translation matrix is pretty straightforward to pull out of the 4x4 and then assign to the tf Transform.

I am confused on the data types and functions to get the rotation working. My plan was to create a 3x3 matrix (data type?), assign it proper values from the 4x4 (method?) that I could then convert into a quaternion (function?), which I then could assign to the transform (I've seen an example of this).

Any help is appreciated. Thank you.

edit retag flag offensive close merge delete

Comments

Here's a related question in python: http://answers.ros.org/question/21298...

dinosaur gravatar image dinosaur  ( 2015-07-06 17:56:21 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted
6

answered 2013-11-21 08:36:38 -0500

TFinleyosu gravatar image

updated 2013-11-21 08:37:13 -0500

Thanks.

I could not get the functions listed in that api to work properly, but it did lead to something else that worked. here is my code that I tested and worked. I know the while (true) is not the best way to publish, but it worked for a test.

  Eigen::Matrix4f Tm;
  Tm <<     0.714191,   -0.54278,   0.441988,  0.0432322,
        0.409255,   0.836069,    0.36542,  0.0571429,
        -0.567861, -0.0800918,   0.819232,    1.22178,
            0,          0,          0,       1; 

  tf::Vector3 origin;
  origin.setValue(static_cast<double>(Tm(0,3)),static_cast<double>(Tm(1,3)),static_cast<double>(Tm(2,3)));

  cout << origin << endl;
  tf::Matrix3x3 tf3d;
  tf3d.setValue(static_cast<double>(Tm(0,0)), static_cast<double>(Tm(0,1)), static_cast<double>(Tm(0,2)), 
        static_cast<double>(Tm(1,0)), static_cast<double>(Tm(1,1)), static_cast<double>(Tm(1,2)), 
        static_cast<double>(Tm(2,0)), static_cast<double>(Tm(2,1)), static_cast<double>(Tm(2,2)));

  tf::Quaternion tfqt;
  tf3d.getRotation(tfqt);

  tf::Transform transform;
  transform.setOrigin(origin);
  transform.setRotation(tfqt);
  while (true) br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "camera_rgb_optical_frame", "lifting_eye"));
edit flag offensive delete link more

Comments

You should use a Matrix4d and drop all the static_cast which makes the code unreadable. You can also use Eigen block operations to remove most of the temporary variables.

Thomas gravatar image Thomas  ( 2013-11-21 17:16:29 -0500 )edit

Thank you for the comment. The output of icp is 4f. I don't think it will work with 4d. If there is an easy conversion please let me know. I don't know what you mean by using Eigen block operations.

TFinleyosu gravatar image TFinleyosu  ( 2013-11-21 17:24:47 -0500 )edit

You can use the built in cast functions of Eigen:

Tm.cast<double>();

Will return an Eigen::Matrix4d

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2019-01-19 06:08:53 -0500 )edit
3

answered 2013-11-20 18:02:02 -0500

Thomas gravatar image

See tf_eigen.h from the tf_conversion package.

I.e. http://docs.ros.org/hydro/api/tf_conversions/html/c++/tf__eigen_8h.html

edit flag offensive delete link more
0

answered 2021-11-23 01:34:03 -0500

majstel gravatar image

updated 2021-12-06 03:43:00 -0500

Here's a simple solution to convert

Eigen::matrix4f eigen_matrix_4f_transform

to

geometry_msgs::TransformStamped tf_stamped

Eigen::Affine3d eigen_affine_transform;
eigen_affine_transform.matrix() = eigen_matrix_4f_transform.cast<double>();
tf_stamped = tf2::eigenToTransform(eigen_affine_transform);

tf_stamped can be later published with

tf::TransformBraodcaster


Remember that you need to set frame_id, child_frame_id nad stamp before publishing

  tf_stamped.header.frame_id = "odom_comb";
  tf_stamped.child_frame_id = "platform";
  tf_stamped.header.stamp = ros::Time::now();
  tf_brodcast_.sendTransform(tf_stamped);
edit flag offensive delete link more

Comments

1

In your answer, where does final_transform_affine come from? is it meant to be the same as eigen_affine_transform, which you assign and then do nothing with?

deedz gravatar image deedz  ( 2021-12-02 19:57:31 -0500 )edit

Yes, the correct input to the function is eigen_affine_transform. Thank you for noticing this bug. I've corrected my answer.

majstel gravatar image majstel  ( 2021-12-06 03:46:25 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2013-11-20 16:56:05 -0500

Seen: 18,352 times

Last updated: Dec 06 '21