Revision history [back]

I'll try and clear up things for you, transforming a pose into a different frame is a standard feature of TF2, you don't have to implement any of the mathematics yourself.

In your code snippet above you're not actually transforming anything, it just publishes the pose data as a TF between two different frames. Since the pose is an inverse of a transform this may well be doing the opposite of what you expect it to. What frame are are the pose messages initially defined in? I would expect it to be the frame of the camera, which frame is this?

The recommended method for your task is to create a tf2_ros::Listener, then to use its attached tf2_ros::Buffer object to transform the pose directly. The Buffer object already has a transform method that will take care of the transformation mathematics for you.

If you use the tf2 listener example here as a starting point, then you replace the tfBuffer.lookupTransform statement within the try catch block with a call to tfBuffer::transform to transform your pose into the frame you want. NOTE this will only work if the original frame of the pose and the new frame are part of the same TF tree.

Also you mention you have three isolated TF trees. This is a problem and is caused either because frames are not names correctly or there are some missing transforms. You'll either want to add some static transform publishers or add a robot model to join these together.

Hope this makes sense.