Naive prediction for SLAM tracker and related changes
- Updates the SLAM tracker to add timestamps to the returned poses and make predictions from the last two tracked poses, so that tracking is not limited to camera fps.
- Updates RealSense and WMR drivers timestamps to monotonic clock.
A note on timestamps for the WMR sources:
Originally, I was doing a single os_monotonic_get_ns()
on start and then adding the same constant offset to each hardware timestamp. However, as suggested by @thaytan, that approach introduces some drift; in some quick tests on an Odyssey+ the timestamps drifted about 0.5ms after 5 minutes. The approach I'm using in this MR looks a lot more stable in comparison although it was an ad-hoc idea (it is an EWMA).
Edited by Mateo de Mayo