Add LiDAR odometry to tf tree

[nachovizzo]

Why do we publish the inverted pose in the tf tree

KISS ICP takes an input point cloud in the lidar frame/robot frame and estimates the pose of the lidar/robot incrementally. This, in tf parlance, will read as (taking the robot frame as example):

KISS ICP, given an input cloud in the base_frame, will estimate the transformation from an odometric frame "odom_lidar" to the "base_link". Basically odom_lidar -> base_link

As a reminder, a wheel odometry node will do something similar. Given the encoder readings, the wheel odometer will produce the transformation from an odometric frame "odom" to the base_link:

Ok, fantastic, but what happens if we now want to publish these two transformations to the tf tree? This is how the final tf tree would look like:

It turns out that the tf2 implementation, by design ( REP-105):

We have chosen a tree representation to attach all coordinate frames in a robot system to each other. Therefore each coordinate frame has one parent coordinate frame, and any number of child coordinate frames.

Which means that the tree you dreamed about is not possible. For two years at KISS-ICP, I needed help figuring out how to solve this issue, and then, we did not publish ANY lidar odometry output to the tf tree. You can't just spin a TF2 listener and look for transformations from the LiDAR odometry system with interpolation, time travel, and all the excellent tools TF2 provides us. In the past, I even tried to hack it around by publishing alias transformations, which was horrible and not working. This topic was already discussed at ros/geometry2#437

The solution: Inject an inverted transformation in the tf tree

Our beloved William realized that if you add not the transformation we compute but the inverse of that (base_link -> odom_lidar), then this is not violating at all the tf conventions:

This has many benefits:

1. We finally have the lidar odometry estimation in the tf tree, so go wild now with your lookups!
2. No more nasty hacks in the visualization pipeline to make sure we can see the clouds (get the kiss map, convert back to lidar frame using kiss_icp.pose().inverse() and put a fake header on this msg. WIP
3. Transparent tf2::lookUpTransform Since tf2_buffer->lookupTransform(target_frame, source_frame, we can do tf2_buffer->lookupTransform("odom_lidar", "base_link), and this will give us the last LiDAR odometry pose. Ture, internally the tf2 buffer will need to invert the transformation that has coded inside, so inverse(tf2_buffer->lookupTransform("base_link", "odom_lidar"), but this is transparent to the user and doesn't even need to know

The only disadvantages I see are

1. Let's compute the lidar odometry pose and then query it in another ROS node (the last one for this example). Then, first we PublishOdom(pose.inverse() and then when querying with the tf return inverse(pose.inverse()) which might introduce some numerical errors
2. If you inspect the /tf and see inside the tf RAW, the transformation you are looking at is inverted, so you can't just plot it with plotjuggler. Let's say you will always need to post-process it and invert it; this information is barely present in the frame_id and child_frame_id inside the tf message; it's clear, but it might be confusing.

Changes

• invert_odom_tf: Introduce a new boolean flag, true by default, to control in the feature whether we want or not to internally publish the inverted transformation through the tf tree. An example would be: the wheel odometry is not part of the tf tree, or we don't have any wheel odometry. Or tf now support multiple parents' IDs? It doesn't matter, but as the proposed solution is slightly hacky, it's fair to guard it with a configurable flag.
• Change rviz2 default "Fixed Frame" to odom_lidar. A true dream come true! We don't even need to know what the frames are in the robot system; 100% of the frames are defined at launch time, and everything is now tightly coupled in the tf tree.
• Now we have a valid tf tree! We can use Rviz for whatever we want; all visualizations are now constantly working, which I did not manage to fix in Fix and improve ROS visualization #285. Now the frames are always displaying the clouds, the odometry message, etc, no matter what fixed frame you pick for visualizing. Of course I set odom_lidar by default

Related discussions

• https://ctu-mrs.github.io/docs/system/frames_of_reference.html#multi-frame-localization-problem
• Allow Multiple Parents at Top Level of TF ros/geometry2#437
• https://discourse.ros.org/t/map-base-odom-as-alternative-for-rep-105-recommended-frame-order/25095/1

Open questions

• I made aeb1cf4 work for my case, for for mainstream kiss-icp is not working (I think yet again I might be missing something). I don't mind keeping this transformation, but it's something that surprised me

Ack

This idea was proposed by William the Great, aka @doisyg , so kudos to him for this kick-ass idea that has been hunting me for months!

[tizianoGuadagnino]

I wish at some point someone will have the will to modify tf in such a way that you can have multiple odometric frames, and just query the one that you need for your specific needs. For now, this is fantastico; big thanks to the ROS black belt 10th dan @doisyg.