interpolationrostf2-ros

Retrieve interpolated transform from tf2_ros::Buffer

Can anyone confirm that the following returns the interpolated transformation from the tf2_ros_buffer_ at the query_stamp?

auto t = tf2_ros_buffer_.lookupTransform(frame_a, frame_b, query_stamp);

And more specifically, is this Cartesian interpolation?

Solution

  • Yes and not exactly, the way that lookupTransform works (as part of tf2) is that it performs spherical linear interpolation (SLERP) between the transform right before and the transform right after the query time (see the original TF paper). This is a trick taken from the computer graphics world that facilitates a constant angular speed model.

    The equation for slerp is below where q_a and q_b are the quaternions representing the frames, q is the output quaternion, t is the ratio of times, and theta is half of the shortest path angle between q_a and q_b

    enter image description here

    Generally the results are used in Cartesian form though.

    You can checkout the source code here (look lookupTransform, transformTF2ToMsg, and the core TF2 interpolation code here).