search

PRBonn / kinematic-icp

A LiDAR odometry pipeline for wheeled mobile robots
https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/kissteam2025icra.pdf
MIT License
199 stars 22 forks source link

Revise timestamping #18

Closed benemer closed 2 weeks ago

benemer commented 2 weeks ago

There are some inconsistencies in our timestamping that this PR addresses to fix.

First, we currently query the TF tree for the odometry from the beginning to the end of the current scan. This is incorrect because the sensor might drop frames, and we, in that case, ignore the odometry during that time.

Second, we assume that the timestamps of the points are absolute, which is not necessarily the case.

We can still not cover one case: if the timestamp of the ROS message is the beginning of the scan and no timestamps are provided for the points. In such a case, there is no way to query the correct odometry from TF because the exact timestamp at the scan's end is unknown.

tizianoGuadagnino commented 2 weeks ago

Introduce the TimeStampsHandler module

I now introduce the TimeStampsHandler module, which implements the logic discussed with @benemer, that is:

  1. If the per-point timestamp is not provided or the `PointCloud2' msg is stamped at the end of the scan, we use the msg stamp itself to query the tf for the odometry or
  2. If the per-point timestamps are provided and the message is stamped at the beginning of the scan (as it should), then we add the scan duration to the message stamp to query the TF for the corresponding wheel odometry.

In both cases, the end stamp of one odometry query will become the begin stamp of the following query.

The only breaking point I see until now is if the scan is stamped at the end and the relative timestamps in the PointField have a negative value. In that case, our motion compensation will break very severely... This shouldn't happen (last famous words).

benemer commented 2 weeks ago

The current implementation misses the combination of stamping at the beginning of the scan and having relative timestamps. Right now, we assume you stamp at the end, even though you can get the duration from the timestamps.

Also, having negative timestamps for de-skewing does not work when using the KISS deskewing because we subtract the mid_pose_timestamp=0.5 from it, see here.