0
$\begingroup$

Rosanswers logo

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.


Originally posted by TFinleyosu on ROS Answers with karma: 385 on 2013-11-20

Post score: 4


Original comments

Comment by dinosaur on 2015-07-06:
Here's a related question in python: http://answers.ros.org/question/212981/rospy-convert-matrix-to-transform/?answer=212985#post-id-212985

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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


Originally posted by Thomas with karma: 4478 on 2013-11-20

This answer was NOT ACCEPTED on the original site

Post score: 3

$\endgroup$
0
$\begingroup$

Rosanswers logo

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

Originally posted by TFinleyosu with karma: 385 on 2013-11-21

This answer was ACCEPTED on the original site

Post score: 6


Original comments

Comment by Thomas on 2013-11-21:
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.

Comment by TFinleyosu on 2013-11-21:
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.

Comment by PeteBlackerThe3rd on 2019-01-19:
You can use the built in cast functions of Eigen:

Tm.cast<double>();

Will return an Eigen::Matrix4d

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.