Optimize integrators, Rviz plugin and add usage examples

[victorreijgwart]

Description

This PR significantly improves the performance of wavemap's measurement integrators and the Rviz plugin. It also introduces several new features, such as utility functions for accelerated map queries and trilinear interpolation in the wavemap library, the option to set the logging verbosity through ROS params in wavemap's ROS server, and options to load and display maps directly from files in the Rviz plugin. Finally, the documentation has been extended with code usage examples.

Type of change

• New feature (non-breaking change which adds functionality)

Detailed summary

New features

• Wavemap library
  • Add map query acceleration utils
  • Add trilinear interpolation utils
• ROS server
  • Make ROS logging level configurable through ROS params
  • Make the number of threads to use configurable through ROS params
  • Add option to TF handler to directly query the most up-to-date transform
  • Add service that resets the map (enabled through ROS params, disabled by default)
• Rviz plugin
  • Add option to load and display maps directly from files
  • Add option to only draw surface voxels
  • Add button to call the wavemap_server's reset map service

Improvements

• Wavemap library
  • Refactor wavemap utils into an extendable toolbox
  • Optimize measurement integration
    • Replace stack with recursion (faster and easier to read)
    • Vectorize batched leaf updater
    • Reduce memory move and copy overheads
    • Simplify measurement model math
    • Postpone image offset error norm root computation
    • Share a single thread pool among all integrators
• ROS server (and ROS interface libraries)
  • Update incremental map transmission to communicate block deletions
  • Add option for multi-threaded block to ROS msg serialization
  • Simplify incremental map transmission logic
  • Consistently use ROS logging in all ROS packages
  • Do not latch map topic
  • Improve example configs
• Rviz plugin
  • General UI improvements
  • Improve block drawing scheduling for faster and smoother rendering
  • Update visualizers to handle deleted blocks
  • Rename "grid" to "voxels" in UI and code for clarity
  • Clean up and optimize visibility query handling
  • Clean up and optimize alpha handling
  • Add Tracy annotations for profiling
• Tools
  • Show documentation preview without using Python HTTP server

Documentation

• Add initial usage examples

Bug fixes

• Wavemap server
  • Fix bug causing delays when transmitting blocks with identical timestamps
• Rviz plugin
  • Fix bug causing delays when drawing blocks with identical timestamps
  • Fix bug causing segfaults when destroying instances of the Rviz plugin

The code optimizations were mostly performed by analyzing the assembly code of the most runtime critical functions, as identified through frame and sampling-based profiling.

Testing

Correctness

• Newly added test suites
  • Map to ROS msg conversions
  • Map query accelerator
  • Trilinear interpolator
• Extended tests
  • Projection model nearest index and offset computation

Performance

All the changes were validated on an AMD laptop (Ryzen 7 PRO 6850U CPU with integrated graphics) and an Intel desktop (i9-9900K CPU and Nvidia RTX 2080 Ti GPU). The changes speed up:

• Measurement integration by ~32% on my laptop and ~10% on my desktop.
• Rviz's frame rate when rendering the map by ~5x on my laptop and 1-2x on my desktop, depending on the map size. On the desktop, the GPU also runs significantly less hot as its compute utilization is reduced from >95% to ~60%.

Checklist:

• My code follows the style guidelines of this project
• I have performed a self-review of my code
• I have commented my code, particularly in hard-to-understand areas
• I have made corresponding changes to the documentation
• Any required changes in dependencies have been committed and pushed

[LionelOtt]

For a future revision it might be good to have some explanations of what the code does in addition to the code itself.

[victorreijgwart]

Yes you're right. I ran out of steam when writing this 🙃 Do you think the explanations would help a lot? If so I can add them now. Otherwise in the next release.

[LionelOtt]

Technically #pragma once is fine to use despite being non-standard as all sane compilers support it nowadays.

[LionelOtt]

Looks good to me.

[victorreijgwart]

Thanks for reviewing @LionelOtt!