Merge pull request #19 from ethz-asl/feature/optimize_performance

Optimize measurement integrator and ROS node performance

libraries/wavemap/CHANGELOG.rst

@@ -2,6 +2,18 @@
 Changelog for package wavemap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+* Annotate code for profiling with Tracy Profiler
+* Switch to custom atan2 in LiDAR projection models
+
+  * Speeds up wavemap by roughly 20% when using LiDAR inputs
+  * No compromise in accuracy (slightly improves AUC, accuracy and recall)
+
+* Minor general optimizations
+* Add option to enable DCHECKs even when not compiling in debug mode
+* Contributors: Victor Reijgwart
+
 1.4.0 (2023-08-30)
 ------------------
 * Document how to configure wavemap

libraries/wavemap/CMakeLists.txt

@@ -3,7 +3,8 @@ project(wavemap)
 
 # Dependencies
 find_package(catkin
-    REQUIRED COMPONENTS glog_catkin eigen_catkin minkindr)
+    REQUIRED COMPONENTS glog_catkin eigen_catkin tracy_catkin minkindr)
+
 if (ENABLE_BENCHMARKING)
   find_package(benchmark REQUIRED)
 endif ()
@@ -22,6 +23,7 @@ catkin_package(
     CATKIN_DEPENDS
     glog_catkin
     eigen_catkin
+    tracy_catkin
     minkindr
     CFG_EXTRAS
     wavemap-extras.cmake)

libraries/wavemap/cmake/wavemap-extras.cmake

@@ -4,6 +4,8 @@
 
 # Add the compiler definitions and options consistently across all wavemap pkgs
 macro (ADD_WAVEMAP_COMPILE_DEFINITIONS_AND_OPTIONS)
+  option(DCHECK_ALWAYS_ON
+      "Enable GLOG DCHECKs even when not compiling in debug mode" OFF)
   option(USE_UBSAN "Compile with undefined behavior sanitizer enabled" OFF)
   option(USE_ASAN "Compile with address sanitizer enabled" OFF)
   option(USE_TSAN "Compile with thread sanitizer enabled" OFF)
@@ -18,6 +20,10 @@ macro (ADD_WAVEMAP_COMPILE_DEFINITIONS_AND_OPTIONS)
     -march=native -Wall -Wextra -Wpedantic -Wsuggest-attribute=const
     -Wno-deprecated-copy -Wno-class-memaccess)
 
+  if (DCHECK_ALWAYS_ON)
+    add_compile_definitions(DCHECK_ALWAYS_ON)
+  endif ()
+
   if (USE_UBSAN)
     add_compile_options(
       -fsanitize=undefined
@@ -38,11 +44,13 @@ macro (ADD_WAVEMAP_COMPILE_DEFINITIONS_AND_OPTIONS)
       -g)
     add_link_options(-fsanitize=undefined)
   endif ()
+
   if (USE_ASAN)
     add_compile_options(-fsanitize=address -fsanitize-address-use-after-scope
                         -fno-omit-frame-pointer -g)
     add_link_options(-fsanitize=address)
   endif ()
+
   if (USE_TSAN)
     add_compile_options(-fsanitize=thread -fno-omit-frame-pointer -g)
     add_link_options(-fsanitize=thread)

libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h

@@ -10,8 +10,10 @@ inline Vector3D OusterProjector::cartesianToSensor(
   const Point2D B_point{
       C_point.head<2>().norm() - config_.lidar_origin_to_beam_origin,
       C_point.z() - config_.lidar_origin_to_sensor_origin_z_offset};
-  const FloatingPoint elevation_angle = std::atan2(B_point.y(), B_point.x());
-  const FloatingPoint azimuth_angle = std::atan2(C_point.y(), C_point.x());
+  const FloatingPoint elevation_angle =
+      approximate::atan2()(B_point.y(), B_point.x());
+  const FloatingPoint azimuth_angle =
+      approximate::atan2()(C_point.y(), C_point.x());
   const FloatingPoint range = B_point.norm();
   return {elevation_angle, azimuth_angle, range};
 }
@@ -61,8 +63,10 @@ inline Vector2D OusterProjector::cartesianToImage(
   const Vector2D B_point{
       C_point.head<2>().norm() - config_.lidar_origin_to_beam_origin,
       C_point.z() - config_.lidar_origin_to_sensor_origin_z_offset};
-  const FloatingPoint elevation_angle = std::atan2(B_point.y(), B_point.x());
-  const FloatingPoint azimuth_angle = std::atan2(C_point.y(), C_point.x());
+  const FloatingPoint elevation_angle =
+      approximate::atan2()(B_point.y(), B_point.x());
+  const FloatingPoint azimuth_angle =
+      approximate::atan2()(C_point.y(), C_point.x());
   return {elevation_angle, azimuth_angle};
 }
 

libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h

@@ -47,8 +47,9 @@ SphericalProjector::imageOffsetsToErrorNorms(
 inline Vector2D SphericalProjector::cartesianToImage(
     const Point3D& C_point) const {
   const FloatingPoint elevation_angle =
-      std::atan2(C_point.z(), C_point.head<2>().norm());
-  const FloatingPoint azimuth_angle = std::atan2(C_point.y(), C_point.x());
+      approximate::atan2()(C_point.z(), C_point.head<2>().norm());
+  const FloatingPoint azimuth_angle =
+      approximate::atan2()(C_point.y(), C_point.x());
   return {elevation_angle, azimuth_angle};
 }
 

libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h

@@ -27,14 +27,14 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator {
         occupancy_map_(std::move(CHECK_NOTNULL(occupancy_map))) {}
 
  private:
-  using BlockList = std::vector<OctreeIndex>;
+  using BlockList = std::vector<HashedChunkedWaveletOctree::BlockIndex>;
 
   const HashedChunkedWaveletOctree::Ptr occupancy_map_;
   ThreadPool thread_pool_;
   std::shared_ptr<RangeImageIntersector> range_image_intersector_;
 
   // Cache/pre-computed commonly used values
-  static constexpr FloatingPoint kNoiseThreshold = 1e-4f;
+  static constexpr FloatingPoint kNoiseThreshold = 1e-3f;
   static constexpr auto kUnitCubeHalfDiagonal =
       constants<FloatingPoint>::kSqrt3 / 2.f;
   const FloatingPoint min_cell_width_ = occupancy_map_->getMinCellWidth();
@@ -54,7 +54,7 @@ class HashedChunkedWaveletIntegrator : public ProjectiveIntegrator {
 
   void updateMap() override;
   void updateBlock(HashedChunkedWaveletOctree::Block& block,
-                   const OctreeIndex& block_index);
+                   const HashedChunkedWaveletOctree::BlockIndex& block_index);
 };
 }  // namespace wavemap
 

libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h

@@ -19,14 +19,14 @@ inline void HashedChunkedWaveletIntegrator::recursiveTester(  // NOLINT
   if (node_index.height == tree_height_) {
     // Get the block
     if (update_type == UpdateType::kPossiblyOccupied) {
-      update_job_list.emplace_back(node_index);
+      update_job_list.emplace_back(node_index.position);
       return;
     }
     if (occupancy_map_->hasBlock(node_index.position)) {
       const auto& block = occupancy_map_->getBlock(node_index.position);
       if (min_log_odds_shrunk_ <= block.getRootScale()) {
         // Add the block to the job list
-        update_job_list.emplace_back(node_index);
+        update_job_list.emplace_back(node_index.position);
       }
     }
     return;

libraries/wavemap/include/wavemap/utils/thread_pool.h

@@ -51,8 +51,8 @@ class ThreadPool {
    * \return std::future containing the result of the computation
    */
   template <typename Callable, typename... Args>
-  std::future<typename std::result_of<Callable(Args...)>::type> add_task(
-      Callable&& callable, Args&&... args);
+  std::future<std::result_of_t<Callable(Args...)>> add_task(Callable&& callable,
+                                                            Args&&... args);
 
  private:
   /**
@@ -79,9 +79,9 @@ class ThreadPool {
 };
 
 template <typename Callable, typename... Args>
-std::future<typename std::result_of<Callable(Args...)>::type>
-ThreadPool::add_task(Callable&& callable, Args&&... args) {
-  using ReturnType = typename std::result_of<Callable(Args...)>::type;
+std::future<std::result_of_t<Callable(Args...)>> ThreadPool::add_task(
+    Callable&& callable, Args&&... args) {
+  using ReturnType = std::result_of_t<Callable(Args...)>;
 
   auto task = std::make_shared<std::packaged_task<ReturnType()>>(
       std::bind(std::forward<Callable>(callable), std::forward<Args>(args)...));

libraries/wavemap/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap</name>
-  <version>1.4.0</version>
+  <version>1.5.0</version>
   <description>Base library for wavemap.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>
@@ -14,6 +14,7 @@
 
   <depend>glog_catkin</depend>
   <depend>eigen_catkin</depend>
+  <depend>tracy_catkin</depend>
   <depend>minkindr</depend>
 
   <test_depend>gtest</test_depend>

libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc

@@ -2,6 +2,8 @@
 
 #include <unordered_set>
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 DECLARE_CONFIG_MEMBERS(HashedChunkedWaveletOctreeConfig,
                       (min_cell_width)
@@ -22,12 +24,14 @@ bool HashedChunkedWaveletOctreeConfig::isValid(bool verbose) const {
 }
 
 void HashedChunkedWaveletOctree::threshold() {
+  ZoneScoped;
   for (auto& [block_index, block] : blocks_) {
     block.threshold();
   }
 }
 
 void HashedChunkedWaveletOctree::prune() {
+  ZoneScoped;
   std::unordered_set<BlockIndex, IndexHash<kDim>> blocks_to_remove;
   for (auto& [block_index, block] : blocks_) {
     block.prune();
@@ -41,6 +45,7 @@ void HashedChunkedWaveletOctree::prune() {
 }
 
 void HashedChunkedWaveletOctree::pruneDistant() {
+  ZoneScoped;
   std::unordered_set<BlockIndex, IndexHash<kDim>> blocks_to_remove;
   for (auto& [block_index, block] : blocks_) {
     if (config_.only_prune_blocks_if_unused_for <
@@ -57,6 +62,7 @@ void HashedChunkedWaveletOctree::pruneDistant() {
 }
 
 size_t HashedChunkedWaveletOctree::getMemoryUsage() const {
+  ZoneScoped;
   // TODO(victorr): Also include the memory usage of the unordered map itself
   size_t memory_usage = 0u;
   for (const auto& [block_index, block] : blocks_) {

libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc

@@ -1,7 +1,10 @@
 #include "wavemap/data_structure/volumetric/hashed_chunked_wavelet_octree_block.h"
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 void HashedChunkedWaveletOctreeBlock::threshold() {
+  ZoneScoped;
   if (getNeedsThresholding()) {
     root_scale_coefficient_ = recursiveThreshold(chunked_ndtree_.getRootChunk(),
                                                  root_scale_coefficient_)
@@ -11,6 +14,7 @@ void HashedChunkedWaveletOctreeBlock::threshold() {
 }
 
 void HashedChunkedWaveletOctreeBlock::prune() {
+  ZoneScoped;
   if (getNeedsPruning()) {
     threshold();
     recursivePrune(chunked_ndtree_.getRootChunk());
@@ -142,6 +146,7 @@ void HashedChunkedWaveletOctreeBlock::addToCellValue(const OctreeIndex& index,
 void HashedChunkedWaveletOctreeBlock::forEachLeaf(
     const BlockIndex& block_index,
     VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn) const {
+  ZoneScoped;
   if (empty()) {
     return;
   }

libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc

@@ -2,6 +2,8 @@
 
 #include <unordered_set>
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 DECLARE_CONFIG_MEMBERS(HashedWaveletOctreeConfig,
                       (min_cell_width)
@@ -21,12 +23,14 @@ bool HashedWaveletOctreeConfig::isValid(bool verbose) const {
 }
 
 void HashedWaveletOctree::threshold() {
+  ZoneScoped;
   for (auto& [block_index, block] : blocks_) {
     block.threshold();
   }
 }
 
 void HashedWaveletOctree::prune() {
+  ZoneScoped;
   std::unordered_set<BlockIndex, IndexHash<kDim>> blocks_to_remove;
   for (auto& [block_index, block] : blocks_) {
     block.prune();
@@ -40,6 +44,7 @@ void HashedWaveletOctree::prune() {
 }
 
 void HashedWaveletOctree::pruneDistant() {
+  ZoneScoped;
   std::unordered_set<BlockIndex, IndexHash<kDim>> blocks_to_remove;
   for (auto& [block_index, block] : blocks_) {
     if (config_.only_prune_blocks_if_unused_for <
@@ -56,6 +61,7 @@ void HashedWaveletOctree::pruneDistant() {
 }
 
 size_t HashedWaveletOctree::getMemoryUsage() const {
+  ZoneScoped;
   // TODO(victorr): Also include the memory usage of the unordered map itself
   size_t memory_usage = 0u;
   for (const auto& [block_index, block] : blocks_) {

libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc

@@ -1,7 +1,10 @@
 #include "wavemap/data_structure/volumetric/hashed_wavelet_octree_block.h"
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 void HashedWaveletOctreeBlock::threshold() {
+  ZoneScoped;
   if (getNeedsThresholding()) {
     root_scale_coefficient_ -=
         recursiveThreshold(ndtree_.getRootNode(), root_scale_coefficient_);
@@ -10,6 +13,7 @@ void HashedWaveletOctreeBlock::threshold() {
 }
 
 void HashedWaveletOctreeBlock::prune() {
+  ZoneScoped;
   if (getNeedsPruning()) {
     threshold();
     recursivePrune(ndtree_.getRootNode());
@@ -18,6 +22,7 @@ void HashedWaveletOctreeBlock::prune() {
 }
 
 void HashedWaveletOctreeBlock::clear() {
+  ZoneScoped;
   root_scale_coefficient_ = Coefficients::Scale{};
   ndtree_.clear();
   last_updated_stamp_ = Time::now();
@@ -104,6 +109,7 @@ void HashedWaveletOctreeBlock::forEachLeaf(
     const BlockIndex& block_index,
     VolumetricDataStructureBase::IndexedLeafVisitorFunction visitor_fn,
     IndexElement termination_height) const {
+  ZoneScoped;
   if (empty()) {
     return;
   }