Apply transformation matrix to existing frame

edit

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.header.stamp = ros::Time::now();
new_frame.header.frame_id = "kinect_frame_name";
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.