Use mechanical odometry as hint for registration

[roncapat]

Many registration algorithms take a roto-traslation "hint" from eg. mechanical odometry. It seems to me in KISS-ICP a guess is made taking last 2 poses:

https://github.com/PRBonn/kiss-icp/blob/eab52a5e0248a573602ec29b3bd1a13f218ed973/cpp/kiss_icp/pipeline/KissICP.cpp#L97-L102

However, this does not allow to inject an external guess source.

What do you think about making possible to the user to provide the guess to the Pipeline API (and in turn , ROS 2 node)?

[tizianoGuadagnino]

Yeah, it will definitely be nice to add some initial guesses for the registration based on wheel odometry or IMU, and we are currently considering adding this feature. At the same time, we want to make this change as clean as possible code-wise, so for now, I can say stay tuned ;)

[roncapat]

Thank you! What about the linked PR? Could it be an initial step to allow users to inject guesses to the Pipeline?

[tizianoGuadagnino]

I'm sorry I forgot to comment on that; adding the initial guess to the C++ API is a good first step. I dream of having some form of abstraction over the module that provides the initial guess; this is most important on the ROS side. Let me include the changes once we have decided how to proceed overall ;)

[nachovizzo]

Adding my two cents here.

While ago, we also had a way (in Python) to inject external odometry sources as the prediction model:

https://github.com/PRBonn/kiss-icp/blob/0ad67b7265cb276a7fe3d106053c8140c55a0979/src/kiss_icp/deskew.py#L51

And here was the "internal" velocity computation https://github.com/PRBonn/kiss-icp/blob/0ad67b7265cb276a7fe3d106053c8140c55a0979/src/kiss_icp/cpp/kiss_icp/Deskew.cpp#L43

But as Tiziano said, we started to look for a more "generic" way to inject this since just using the raw velocities from an IMU (as an example) was, of course, worse than using the constant velocity model.

We will notify you as long as we find internal peace regarding this, but in the meantime, I will close the issue and the PR that comes with it. Thanks a lot for the contribution, though!

PS: If you have some data where you can show somehow that this makes a big difference, then I will re-open the PR and merge the changes (if that's ok to you). Please feel free to comment down, we are looking forward your opinion

[roncapat]

Ok, thanks :)

We branched privately for now listening for base_link position estimate coming from a robot_localization filter processing mechanical odometry + IMU.

IMHO, in ROS2, there are two options to provide out-of-the-box guesses from external sources:

• listening to odometry messages from an external source: this has the problem you should do manual buffering of odometry messages and pose interpolation to get pose estimates at each instant you receive a new point cloud in kiss-icp node callback (based on cloud timestamp)
• having TF2 doing it for you (so you can query at any time instant, with interpolation and buffering under the TF2 hood)

But if you understand correctly, you are planning instead to use IMU measures internally (like an integrated visual-inertial solution). I understand this approach, but it may be less general than accepting a sort of generic position input from other nodes as drafted above.

[saadehmd]

@roncapat . If you don't mind sharing this information from your implementation, which of the above two interpolation methods, did you end up incorporating in your own branch? I quickly tried the (tf2_ros::Buffer + tf2_ros::TransformListener) combination that original authors already declared in the OdometryServer.cpp. But somehow, tf2_buffer within a rclcpp::Node always lags behind latest actually available transforms. So much so that, i have the external_odometry source publishing at 6 times that of sensor_frame_rate (a RADAR in my case which publishes at a mere 4Hz and a fused odmetry at 24 Hz.) When i compare the latest stamps of sensor data and odom data thru CLI tools like ros2 topic echo or ros2 run tf2_echo world odom, the latest available stamp is always few milliseconds newer than the sensor stamp. Yet, getting this same odom -> world transform in the node (at the latest sensor stamp) results in "Look up would require extrapolation into future error". So, now i'm thinking whether it's worth the effort, to actually go on and just implement my own Odometry buffer and do the interpolation manually as well.

The pictures below have the layout:-

top_right: KISS_ICP node with LookUpTransform(world -> odom) Bottom_left: sensor stamp Bottom_right: odom stamp

Another detailed image: bottom row ( starting from left to right):- [sensor stamp, odom stamp, clock, tf2_echo world -> odom]

[saadehmd]

Update: Ended up using a slightly higher timeout in lookUpTransform() and it seem to work fine now. But still curious to know what method was used by others for handling external odom.