Apply transformation matrix to existing frame

As the title says, I have a frame, which comes by default in the Kinect, and then I calculated a 4x4 transformation matrix. How can I apply it to the existing frame?

edit retag close merge delete

Sort by ยป oldest newest most voted

The easiest method is to create a new frame that is the transformed Kinect frame. You can then use that frame throughout your system.

You can create a new frame from your transformation matrix and broadcast it using tf2. See this tutorial for how to broadcast a transform. If the transform will never change, you can also follow this tutorial to broadcast a static frame. (There's also a tutorial specifically about adding a new frame, but it's much the same as the broadcast frame tutorial.)

To create the frame from your transformation matrix, you need to convert it to tf2's Matrix3x3 type for the rotation part and a Vector3 for the translation part. From that, you can construct a tf2::Transform object, passing the matrix into the constructor. That can then be converted to the message data type for broadcasting using the tf2::fromMsg() function

tf2::Matrix3x3 tm(xx, xy, xz, yx, yy, yz, zx, zy, zz);
tf2::Vector3 translation(x, y, z);
tf2::Transform trans(tm, translation);
geometry_msgs::TransformStamped new_frame;
new_frame.child_frame_id = "transformed_kinect";
new_frame.transform = tf2::toMsg(trans);


If you know the Euler rotation angles originally used to create the transformation matrix, then another option is to convert them to a quaternion and put that into the message's transform member.

On the other hand, if you want to use the transformed frame locally only, you can get the pose of the Kinect frame, convert it to a tf2::Transform object using the tf2::fromMsg() function. This will convert a geometry_msgs::Pose to a tf2::Transform. You can then convert your 4x4 transformation matrix to another tf2::Transform object, multiply them together to get the resulting transformation from the origin of the frame you original got the Kinect frame relative to, and that's the pose of your transformed Kinect frame.

more

Thank you very much for your detailed response. I'm trying to apply everything you told me but I can't help but notice that the last part is ideal for me. Isn't there a way to subscribe to that tf, save it somewhere and multiply it with the transformation matrix and then publish the new one?

( 2018-04-20 03:20:54 -0600 )edit
1

That's what the first part does.

( 2018-04-20 08:35:12 -0600 )edit

Thanks, it worked perfectly ^^

( 2018-04-20 10:11:44 -0600 )edit