Improve 2D velocity estimation to be less fragile to poor data timing

[SirVer]

This is the underlying issue for #233 and for the tuning problems in cartographer-project/cartographer_ros#317.

Right now, if two scans are very close in time, velocity estimation will become very big and mess up position quite profoundly.

[SirVer]

Offensive code is here: https://github.com/googlecartographer/cartographer/blob/b4b83405ce4009ea0c1ac22c7ab9edeeb9d48a42/cartographer/mapping_2d/local_trajectory_builder.cc#L204

[Entropy512]

This is probably also the cause of the issues @ojura had in #193 .

This made me take a second look at the lms1xx driver (both ojura and I are using lms1xx devices at the moment) - it marks the timestamp as being when the message was processed by the driver - https://github.com/clearpathrobotics/LMS1xx/blob/master/src/LMS1xx_node.cpp#L170 - in this case, unfortunately, it ignores the timestamp provided by the scanner with each scan.

For LMS1xx users, the solution appears to be a bit of driver rework so it uses the laser's timestamps as a time reference. For other people - I'm not sure if the best solution is an intermediary filter node (which massages timestamps to roughly track the average scan time - possibly explicitly telling it the expected scan rate?) or to implement some sort of timestamp filtering within Cartographer?

@ojura - for your existing bags - maybe you can do a timing analysis of the inter-scan timestamp deltas, if there's significant jitter you might be able to implement something that filters the timestamps to keep them close to the expected 50 Hz?

[SirVer]

I think it is wrong to add a guesstimate inside Cartographer. It's not Cartographer's to second guess the data - it will do the best possible job it can, but garbage in - garbage out. Also, any filtering would probably hurt users that have really good timing.

I think the filtering node is the way to go. And timing of the scans should be simple too: Lasers are pretty precise in their timing - if they promise a laser shot every 10ms, this will be accurate. It is usually pretty okay to just take a starting time stamp and trust the laser that its timing stays solid. This will be way better than the system time of processing the packets.

While we talk about data representation: another problem is ROS'es data format, most notably sensor_msgs/LaserScan which assumes that the laser rotates evenly - which is basically never true, and certainly not if your spin direction is not aligned with gravity. The correct data format is a tuple of (azimut, time, distance) + intensity or whatever data the sensor additionally provides.

[ojura]

Hi everyone! I think deltas are okay in my bag, IIRC they are ranging from 18-22 milliseconds (should be 20, i.e. 50 Hz).

@Entropy512 now that you mention it, I tackled this exact issue a few months ago when updating the ROS driver of a similar scanner, Sick S300. I wrote a timesync class which filters the difference between the system clock and the sensor clock, thereby eliminating system clock jitter -- it trusts the sensor clock locally, and globally, it gently pulls the timestamp to the system clock, so there's no drift. If you want to play with it, the class is available as a ROS package in this repo. For documentation, check the headers, and here is a commit in which the class is used in a driver node for the S300 laser scanner -- it's not much work. The main thing is the call to the sync() method (line 159), to which you pass the system clock and the sensor clock (which is, in that example, calculated by multiplying the sensor sample rate with the message count).

The filter works by first filtering the system-sensor timestamp difference with a median filter (some day I am planning to replace this step with a convex hull filter like here to eliminate the delay caused by the median filter). Then the resulting signal is again filtered with a simple second-order exponential filter (Holt-Winters) which estimates the slope of the difference, i.e. the clock drift. To obtain the best results, you have to tune the filter parameters; there's also a Matlab version in the util folder which plots the results to aid you in tuning. A cool feature is that the sensor and the system clock are published to a debug topic, so if you bag these it's possible to recalculate the stamps in a bag with different filtering parameters (although I haven't written a script which will do that...yet). If you want to play with this, be my guest :-)

[ahundt]

It sounds like something like TicSync/TriggerSync (pdf) might make sense here. More accurate time estimates would ensure the velocity estimates are accurate too.

ROS code:
https://github.com/englishar/trigger_sync

[ojura]

Good to know! That's the same approach I took, just done properly. :-) Although the problem isn't only in stamp synchronization - scan matcher noise can also break 2D velocity estimation.

[BrannonKing]

It appears that we recently added a time filter window of 1ms for IMU. I'm not sure that is enough smoothing, but it might do something. I think that parameter should get exposed for configuration.