Latency troubleshooting with rtabmap-ros

[Alan]

Hi,

I am mapping with an L515 lidar on rtabmap-ros and I am encountering some mapping latency that I can't really explain. Odometry is done with ICP via libpointmatcher and I am getting consistently ~0.03s update frequency

[ INFO] [1600664613.655947447]: Odom: ratio=0.046694, std dev=0.015367m|0.015367rad, update time=0.033356s

For some reason however the map will be delayed up to several seconds, even though it looks like the RTABMap node is running at 3 hz, faster than my 2 hz detection rate:

[ INFO] [1600664613.892001514]: rtabmap (140): Rate=0.50s, Limit=0.100s, RTAB-Map=0.3242s, Maps update=0.0025s pub=0.0000s (local map=72, WM=72)

As you can see I have set the TimeThr to 100 ms to try and force the mapping node closer to real time, but this doesn't appear to have an effect.
The rest of my settings are essentially as described in the launch file in this github comment:
https://github.com/introlab/rtabmap/issues/574#issuecomment-689149729
I have also set General/decimation0 to 20 to try and speed things up.

Running on ROS melodic with a Ryzen 9 4900HS CPU. I was previously able to map with the L515 on this computer using ICP and F2M without any lag until the map got too large, but I didn't back up my old .launch file. Now I have significant latency from the beginning, though I am not sure what has changed. Do you have any suggestions for tracking down the bottleneck?
Thanks!

[matlabbe]

Hi,

Rtabmap/TimeThr won't make the processing time of a single frame going faster. If one frame takes 300 ms to process and Rtabmap/TimeThr is set to 100 ms, it will just make rtabmap to keep a very low number of nodes in the map.

The latency should indeed be 300 ms, not several seconds. Is it rtabmapviz lagging or rtabmap node? If it is rtabmapviz, look at the parameters in Preferences->3D rendering to decimate or voxelize more the point clouds.

Can you share a database of when the latency is starting to appear?

[Alan]

Suddenly my latency issues stopped and I am close to having very nice ICP odometry with the L515. I am still trying to troubleshoot some problems.

In the linked video here, I am resuming after triggering a new map and resetting odometry. You will see that the first frame added to the map has a dense amount of points added to the map, and since I am doing F2M odometry, the pose is very stable when I have the camera facing that first section of map. For some reason subsequent frames do not add nearly as many points, except for on rare occasion. I can't figure out what is causing this behavior.
https://youtu.be/lz3Xp3H-fC4

 Here is my rqt_graph to give you a better idea of my setup:


I think if it updated the map with the full scan every frame it would be working perfectly. Any idea what I did wrong?

A

[matlabbe]

To better see the odometry local map, in Preferences->3D Rendering, uncheck Map's Cloud and Scan rendering. Keep only Odometry's scan rendering. Then click in 3D Map view, tap on "2" key on your keyboard. The color layout will change to Pink=latest scan and Blue=Local odometry map.

With F2M, you can play with OdomF2M/ScanMaxSize (default 2000) to keep a larger local map, Odom/ScanKeyFrameThr (default 0.9) to trigger less or more often an update of the local odom map. In general I would  set OdomF2M/ScanSubtractRadius to same value used to voxelize the scans. Make sure you voxelize your scans (or set Icp/VoxelSize).

In your case, the odometry got lost when looking at the flat wall. This is an issue with ICP, make sure to have always geometry in the field of view of the camera.

cheers,
Mathieu

[Alan]

Thank you. It was in fact the ScanKeyFrameThr that was set too high. I had it at 0.3 but setting it to 0 fixed it.