Trajectory publishing is slow

cartographer-project / cartographer_ros

Provides ROS integration for Cartographer.

Apache License 2.0

1.67k stars 1.21k forks source link

Trajectory publishing is slow #762

Open wohe opened 6 years ago

wohe commented 6 years ago

When running the 3D demo at https://google-cartographer-ros.readthedocs.io/en/latest/#running-the-demos, performance is much worse as long as trajectory visualization is enabled.

On my machine wall clock time is 1321.99 s with the trajectory shown, 1009.06 s when it is hidden. Command line:

roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=b3-2016-04-05-14-14-00.bag

Also bandwidth of /trajectory_node_list is huge, 17 MB/s at the end, compared to about 3 MB/s for /scan_matched_points2.

ojura commented 6 years ago

Maybe this deserves an rfc, but here are some ideas:

handle displaying the trajectory in a cartographer_rviz plugin (either in the submaps plugin, or a new plugin)
stream via a “trajectory updates” topic only incremental local slam results (newly inserted nodes), as well as a trajectory version number which increases whenever optimization is run
if the version number has increased, the plugin knows it should request a complete new version of the trajectory via a service, similar to submaps

A quick fix would be simply reducing the trajectory publishing period. @wohe what do you think?

ojura commented 6 years ago

@wohe ping, would love to have some discussion on this. Do you want me to write an RFC?

wohe commented 6 years ago

Since this ideally would just improve performance and not introduce new concepts I'm not sure it needs an RFC. It definitely needs one or more PRs.

Adding a new plugin is possible but might not be necessary, given that the RViz markers to my understanding already allow sending updates.

I see two easy ways to improve performance without adding an RViz plugin:

Compression: Only add vertices to a trajectory if there has been enough movement and remove vertices which leave the resulting polygonal chain almost unchanged.
Send diffs: Use the functionality provided by RViz markers to send updates: Split the trajectory into smaller pieces (e.g. every 100 vertices or so) and only send updates for the pieces which need it (new vertex was added, or an optimization result changed it)

ojura commented 6 years ago

Is signalling that optimization changed the trajectory a new concept for libcartographer? AFAIK we are not signalling this anywhere to the library user (right now they would have to compare older trajectory node poses by themselves to see if they changed).

E.g. we could introduce a GlobalSlamResult callback, similar how we have a callback for LocalSlamResult.

MichaelGrupp commented 4 years ago

@wohe do you plan to work on this?

One thing that is also affecting performance are high publish frequencies. The trajectory marker period is currently set to 30e-3, which is probably not needed. https://github.com/cartographer-project/cartographer_ros/blob/6296d41cd5a2a82bcb9d23e628ce02f0435c9006/cartographer_ros/configuration_files/backpack_3d.lua#L38

wohe commented 3 years ago

@MichaelGrupp I would not say I have a plan to work on this. Assigned it to myself so it is not forgotten. Not sure whether/when I will find the time, though.