diff --git a/libraries/wavemap/CHANGELOG.rst b/libraries/wavemap/CHANGELOG.rst
index 058aa7da5..bd01777be 100644
--- a/libraries/wavemap/CHANGELOG.rst
+++ b/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
diff --git a/libraries/wavemap/CMakeLists.txt b/libraries/wavemap/CMakeLists.txt
index 7ddd2d087..99cce19dc 100644
--- a/libraries/wavemap/CMakeLists.txt
+++ b/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)
diff --git a/libraries/wavemap/cmake/wavemap-extras.cmake b/libraries/wavemap/cmake/wavemap-extras.cmake
index b4a5cc3d1..2270a3571 100644
--- a/libraries/wavemap/cmake/wavemap-extras.cmake
+++ b/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)
diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h
index f4da5624c..82e25168e 100644
--- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/ouster_projector_inl.h
+++ b/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};
 }
 
diff --git a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h b/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h
index 887a5a859..64aedfc6f 100644
--- a/libraries/wavemap/include/wavemap/integrator/projection_model/impl/spherical_projector_inl.h
+++ b/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};
 }
 
diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h
index 22fd6aa9f..e53ee900a 100644
--- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.h
+++ b/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
 
diff --git a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h b/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h
index f9fd9b61d..1f3c283d7 100644
--- a/libraries/wavemap/include/wavemap/integrator/projective/coarse_to_fine/impl/hashed_chunked_wavelet_integrator_inl.h
+++ b/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;
diff --git a/libraries/wavemap/include/wavemap/utils/thread_pool.h b/libraries/wavemap/include/wavemap/utils/thread_pool.h
index 669b62856..b58ef10ba 100644
--- a/libraries/wavemap/include/wavemap/utils/thread_pool.h
+++ b/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)...));
diff --git a/libraries/wavemap/package.xml b/libraries/wavemap/package.xml
index a0a02a2ea..423e4c3a3 100644
--- a/libraries/wavemap/package.xml
+++ b/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="victorr@ethz.ch">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>
diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc
index ed47ce704..2a8204ebe 100644
--- a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc
+++ b/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_) {
diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc
index 7fe0af9e3..fee64f86e 100644
--- a/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc
+++ b/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;
   }
diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc
index fccf56193..9167368cd 100644
--- a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc
+++ b/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_) {
diff --git a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc b/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc
index 8eed78e04..eb7cebfea 100644
--- a/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc
+++ b/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;
   }
diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc
index 3493ec145..bfae287ec 100644
--- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc
+++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_chunked_wavelet_integrator.cc
@@ -2,32 +2,41 @@
 
 #include <stack>
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 void HashedChunkedWaveletIntegrator::updateMap() {
+  ZoneScoped;
   // Update the range image intersector
-  range_image_intersector_ = std::make_shared<RangeImageIntersector>(
-      posed_range_image_, projection_model_, *measurement_model_,
-      config_.min_range, config_.max_range);
+  {
+    ZoneScopedN("updateRangeImageIntersector");
+    range_image_intersector_ = std::make_shared<RangeImageIntersector>(
+        posed_range_image_, projection_model_, *measurement_model_,
+        config_.min_range, config_.max_range);
+  }
 
   // Find all the indices of blocks that need updating
   BlockList blocks_to_update;
-  const auto [fov_min_idx, fov_max_idx] =
-      getFovMinMaxIndices(posed_range_image_->getOrigin());
-  for (const auto& block_index :
-       Grid(fov_min_idx.position, fov_max_idx.position)) {
-    recursiveTester(OctreeIndex{fov_min_idx.height, block_index},
-                    blocks_to_update);
+  {
+    ZoneScopedN("selectBlocksToUpdate");
+    const auto [fov_min_idx, fov_max_idx] =
+        getFovMinMaxIndices(posed_range_image_->getOrigin());
+    for (const auto& block_index :
+         Grid(fov_min_idx.position, fov_max_idx.position)) {
+      recursiveTester(OctreeIndex{fov_min_idx.height, block_index},
+                      blocks_to_update);
+    }
   }
 
   // Make sure the to-be-updated blocks are allocated
   for (const auto& block_index : blocks_to_update) {
-    occupancy_map_->getOrAllocateBlock(block_index.position);
+    occupancy_map_->getOrAllocateBlock(block_index);
   }
 
   // Update it with the threadpool
   for (const auto& block_index : blocks_to_update) {
     thread_pool_.add_task([this, block_index]() {
-      auto& block = occupancy_map_->getBlock(block_index.position);
+      auto& block = occupancy_map_->getBlock(block_index);
       updateBlock(block, block_index);
     });
   }
@@ -56,7 +65,9 @@ HashedChunkedWaveletIntegrator::getFovMinMaxIndices(
 }
 
 void HashedChunkedWaveletIntegrator::updateBlock(
-    HashedChunkedWaveletOctree::Block& block, const OctreeIndex& block_index) {
+    HashedChunkedWaveletOctree::Block& block,
+    const HashedChunkedWaveletOctree::BlockIndex& block_index) {
+  ZoneScoped;
   block.setNeedsPruning();
   block.setLastUpdatedStamp();
 
@@ -70,7 +81,7 @@ void HashedChunkedWaveletIntegrator::updateBlock(
   };
   std::stack<StackElement, std::vector<StackElement>> stack;
   stack.emplace(StackElement{
-      block.getRootChunk(), block_index, 0,
+      block.getRootChunk(), OctreeIndex{tree_height_, block_index}, 0,
       HashedChunkedWaveletOctreeBlock::Transform::backward(
           {block.getRootScale(), block.getRootChunk().nodeData(0u)}),
       block.getRootChunk().nodeHasAtLeastOneChild(0u)});
@@ -107,7 +118,31 @@ void HashedChunkedWaveletIntegrator::updateBlock(
       }
     }
 
-    // Evaluate stack element's active child
+    // If we're at the leaf level, directly compute the update for all children
+    if (stack.top().parent_node_index.height ==
+        config_.termination_height + 1) {
+      DCHECK_EQ(stack.top().next_child_idx, 0);
+      for (NdtreeIndexRelativeChild relative_child_idx = 0;
+           relative_child_idx < OctreeIndex::kNumChildren;
+           ++relative_child_idx) {
+        const OctreeIndex node_index =
+            stack.top().parent_node_index.computeChildIndex(relative_child_idx);
+        FloatingPoint& node_value =
+            stack.top().child_scale_coefficients[relative_child_idx];
+        const Point3D W_node_center =
+            convert::nodeIndexToCenterPoint(node_index, min_cell_width_);
+        const Point3D C_node_center =
+            posed_range_image_->getPoseInverse() * W_node_center;
+        const FloatingPoint sample = computeUpdate(C_node_center);
+        node_value = std::clamp(sample + node_value, min_log_odds_padded_,
+                                max_log_odds_padded_);
+      }
+      stack.top().next_child_idx = OctreeIndex::kNumChildren;
+      continue;
+    }
+
+    // Otherwise, handle the stack element's children one by one
+    // Get the active child
     const NdtreeIndexRelativeChild current_child_idx =
         stack.top().next_child_idx;
     ++stack.top().next_child_idx;
@@ -120,20 +155,7 @@ void HashedChunkedWaveletIntegrator::updateBlock(
         stack.top().child_scale_coefficients[current_child_idx];
     DCHECK_GE(node_index.height, 0);
 
-    // If we're at the leaf level, directly update the node
-    if (node_index.height == config_.termination_height) {
-      const Point3D W_node_center =
-          convert::nodeIndexToCenterPoint(node_index, min_cell_width_);
-      const Point3D C_node_center =
-          posed_range_image_->getPoseInverse() * W_node_center;
-      const FloatingPoint sample = computeUpdate(C_node_center);
-      node_value = std::clamp(sample + node_value, min_log_odds_padded_,
-                              max_log_odds_padded_);
-      continue;
-    }
-
-    // Otherwise, test whether the current node is fully occupied;
-    // free or unknown; or fully unknown
+    // Test whether it is fully occupied; free or unknown; or fully unknown
     const AABB<Point3D> W_cell_aabb =
         convert::nodeIndexToAABB(node_index, min_cell_width_);
     const UpdateType update_type =
diff --git a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc
index 33957e2df..96f1e025d 100644
--- a/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc
+++ b/libraries/wavemap/src/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.cc
@@ -2,21 +2,30 @@
 
 #include <stack>
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 void HashedWaveletIntegrator::updateMap() {
+  ZoneScoped;
   // Update the range image intersector
-  range_image_intersector_ = std::make_shared<RangeImageIntersector>(
-      posed_range_image_, projection_model_, *measurement_model_,
-      config_.min_range, config_.max_range);
+  {
+    ZoneScopedN("updateRangeImageIntersector");
+    range_image_intersector_ = std::make_shared<RangeImageIntersector>(
+        posed_range_image_, projection_model_, *measurement_model_,
+        config_.min_range, config_.max_range);
+  }
 
   // Find all the indices of blocks that need updating
   BlockList blocks_to_update;
-  const auto [fov_min_idx, fov_max_idx] =
-      getFovMinMaxIndices(posed_range_image_->getOrigin());
-  for (const auto& block_index :
-       Grid(fov_min_idx.position, fov_max_idx.position)) {
-    recursiveTester(OctreeIndex{fov_min_idx.height, block_index},
-                    blocks_to_update);
+  {
+    ZoneScopedN("selectBlocksToUpdate");
+    const auto [fov_min_idx, fov_max_idx] =
+        getFovMinMaxIndices(posed_range_image_->getOrigin());
+    for (const auto& block_index :
+         Grid(fov_min_idx.position, fov_max_idx.position)) {
+      recursiveTester(OctreeIndex{fov_min_idx.height, block_index},
+                      blocks_to_update);
+    }
   }
 
   // Make sure the to-be-updated blocks are allocated
@@ -58,6 +67,7 @@ HashedWaveletIntegrator::getFovMinMaxIndices(
 
 void HashedWaveletIntegrator::updateBlock(HashedWaveletOctree::Block& block,
                                           const OctreeIndex& block_index) {
+  ZoneScoped;
   HashedWaveletOctreeBlock::NodeType& root_node = block.getRootNode();
   HashedWaveletOctreeBlock::Coefficients::Scale& root_node_scale =
       block.getRootScale();
diff --git a/libraries/wavemap/src/integrator/projective/projective_integrator.cc b/libraries/wavemap/src/integrator/projective/projective_integrator.cc
index 4dad8dc1a..646825620 100644
--- a/libraries/wavemap/src/integrator/projective/projective_integrator.cc
+++ b/libraries/wavemap/src/integrator/projective/projective_integrator.cc
@@ -1,5 +1,7 @@
 #include "wavemap/integrator/projective/projective_integrator.h"
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 DECLARE_CONFIG_MEMBERS(ProjectiveIntegratorConfig,
                       (min_range)
@@ -21,6 +23,7 @@ bool ProjectiveIntegratorConfig::isValid(bool verbose) const {
 
 void ProjectiveIntegrator::integratePointcloud(
     const PosedPointcloud<Point<3>>& pointcloud) {
+  ZoneScoped;
   if (!isPointcloudValid(pointcloud)) {
     return;
   }
@@ -30,12 +33,14 @@ void ProjectiveIntegrator::integratePointcloud(
 
 void ProjectiveIntegrator::integrateRangeImage(
     const PosedImage<>& range_image) {
+  ZoneScoped;
   importRangeImage(range_image);
   updateMap();
 }
 
 void ProjectiveIntegrator::importPointcloud(
     const PosedPointcloud<>& pointcloud) {
+  ZoneScoped;
   // Reset the posed range image and the beam offset image
   posed_range_image_->resetToInitialValue();
   posed_range_image_->setPose(pointcloud.getPose());
@@ -74,6 +79,7 @@ void ProjectiveIntegrator::importPointcloud(
 
 void ProjectiveIntegrator::importRangeImage(
     const PosedImage<>& range_image_input) {
+  ZoneScoped;
   *posed_range_image_ = range_image_input;
   beam_offset_image_->resetToInitialValue();
 }
diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc
index 165e7fa3d..70a2f6d11 100644
--- a/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc
+++ b/libraries/wavemap/test/src/integrator/projection_model/test_image_projectors.cc
@@ -176,7 +176,9 @@ TYPED_TEST(Image2DProjectorTypedTest, Conversions) {
         projector.sensorToCartesian(sensor_coordinates);
     const FloatingPoint range = C_point.norm();
 
-    EXPECT_LE((C_point_roundtrip - C_point).norm(), kEpsilon * (1.f + range))
+    constexpr FloatingPoint kNoiseTolerance = 1e-5f;
+    EXPECT_LE((C_point_roundtrip - C_point).norm(),
+              kNoiseTolerance * (1.f + range))
         << "Original point was " << EigenFormat::oneLine(C_point)
         << " with norm " << range << ", but after round trip it became "
         << EigenFormat::oneLine(C_point_roundtrip)
diff --git a/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc b/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc
index 86dbd19ab..963e09488 100644
--- a/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc
+++ b/libraries/wavemap/test/src/integrator/projection_model/test_spherical_projector.cc
@@ -49,7 +49,8 @@ TEST_F(SphericalProjectorTest, InitializationAndAccessors) {
 }
 
 TEST_F(SphericalProjectorTest, CellToBeamAngles) {
-  constexpr FloatingPoint kMaxAcceptableAngleError = 2.f * kEpsilon;
+  constexpr FloatingPoint kNoiseTolerance = 1e-5f;
+  constexpr FloatingPoint kMaxAcceptableAngleError = 2.f * kNoiseTolerance;
   const auto projector = getRandomProjectionModel();
   for (int repetition = 0; repetition < 1000; ++repetition) {
     const Point3D C_point = getRandomPoint<3>();
@@ -65,7 +66,7 @@ TEST_F(SphericalProjectorTest, CellToBeamAngles) {
     const Point3D C_point_round_trip =
         projector.sensorToCartesian(sensor_coordinates);
     ASSERT_LE((C_point - C_point_round_trip).norm(),
-              kEpsilon * (1.f + sensor_coordinates[2]));
+              kNoiseTolerance * (1.f + sensor_coordinates[2]));
 
     // Compute "ground truth" using double precision
     const Point3D C_from_closest_pixel = projector.sensorToCartesian(
diff --git a/libraries/wavemap_io/CHANGELOG.rst b/libraries/wavemap_io/CHANGELOG.rst
index e5fd9dc3d..3f0aff3b9 100644
--- a/libraries/wavemap_io/CHANGELOG.rst
+++ b/libraries/wavemap_io/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package wavemap_io
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+* Improve error messages when reading/writing a file fails
+* Contributors: Victor Reijgwart
+
 1.4.0 (2023-08-30)
 ------------------
 
diff --git a/libraries/wavemap_io/include/wavemap_io/file_conversions.h b/libraries/wavemap_io/include/wavemap_io/file_conversions.h
index 316539ed0..2c2fcaca9 100644
--- a/libraries/wavemap_io/include/wavemap_io/file_conversions.h
+++ b/libraries/wavemap_io/include/wavemap_io/file_conversions.h
@@ -1,7 +1,7 @@
 #ifndef WAVEMAP_IO_FILE_CONVERSIONS_H_
 #define WAVEMAP_IO_FILE_CONVERSIONS_H_
 
-#include <string>
+#include <filesystem>
 
 #include <wavemap/data_structure/volumetric/volumetric_data_structure_base.h>
 
@@ -9,8 +9,8 @@
 
 namespace wavemap::io {
 bool mapToFile(const VolumetricDataStructureBase& map,
-               const std::string& file_path);
-bool fileToMap(const std::string& file_path,
+               const std::filesystem::path& file_path);
+bool fileToMap(const std::filesystem::path& file_path,
                VolumetricDataStructureBase::Ptr& map);
 }  // namespace wavemap::io
 
diff --git a/libraries/wavemap_io/package.xml b/libraries/wavemap_io/package.xml
index e81172237..39bf62f16 100644
--- a/libraries/wavemap_io/package.xml
+++ b/libraries/wavemap_io/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_io</name>
-  <version>1.4.0</version>
+  <version>1.5.0</version>
   <description>(De)serialization of wavemap types to files.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/libraries/wavemap_io/src/file_conversions.cc b/libraries/wavemap_io/src/file_conversions.cc
index 384efb214..2e45ae48e 100644
--- a/libraries/wavemap_io/src/file_conversions.cc
+++ b/libraries/wavemap_io/src/file_conversions.cc
@@ -4,14 +4,19 @@
 
 namespace wavemap::io {
 bool mapToFile(const VolumetricDataStructureBase& map,
-               const std::string& file_path) {
-  CHECK(!file_path.empty());
+               const std::filesystem::path& file_path) {
+  if (file_path.empty()) {
+    LOG(WARNING)
+        << "Could open file for writing. Specified file path is empty.";
+    return false;
+  }
 
   // Open the file for writing
   std::ofstream file_ostream(file_path,
                              std::ofstream::out | std::ofstream::binary);
   if (!file_ostream.is_open()) {
-    LOG(WARNING) << "Could not open file '" << file_path << "' for writing.";
+    LOG(WARNING) << "Could not open file " << file_path
+                 << " for writing. Error: " << strerror(errno);
     return false;
   }
 
@@ -25,22 +30,26 @@ bool mapToFile(const VolumetricDataStructureBase& map,
   return static_cast<bool>(file_ostream);
 }
 
-bool fileToMap(const std::string& file_path,
+bool fileToMap(const std::filesystem::path& file_path,
                VolumetricDataStructureBase::Ptr& map) {
-  CHECK(!file_path.empty());
+  if (file_path.empty()) {
+    LOG(WARNING)
+        << "Could not open file for reading. Specified file path is empty.";
+    return false;
+  }
 
   // Open the file for reading
   std::ifstream file_istream(file_path,
                              std::ifstream::in | std::ifstream::binary);
   if (!file_istream.is_open()) {
-    LOG(WARNING) << "Could not open file '" << file_path << "' for reading.";
+    LOG(WARNING) << "Could not open file " << file_path
+                 << " for reading. Error: " << strerror(errno);
     return false;
   }
 
   // Deserialize from bytestream
   if (!streamToMap(file_istream, map)) {
-    LOG(WARNING) << "Failed to parse map proto from file '" << file_path
-                 << "'.";
+    LOG(WARNING) << "Failed to parse map from file " << file_path << ".";
     return false;
   }
 
diff --git a/ros/wavemap_msgs/CHANGELOG.rst b/ros/wavemap_msgs/CHANGELOG.rst
index b3b91ce37..7f465e134 100644
--- a/ros/wavemap_msgs/CHANGELOG.rst
+++ b/ros/wavemap_msgs/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+
 1.4.0 (2023-08-30)
 ------------------
 
diff --git a/ros/wavemap_msgs/package.xml b/ros/wavemap_msgs/package.xml
index 67f60284a..359191447 100644
--- a/ros/wavemap_msgs/package.xml
+++ b/ros/wavemap_msgs/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_msgs</name>
-  <version>1.4.0</version>
+  <version>1.5.0</version>
   <description>Message definitions for wavemap's ROS interfaces.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/ros/wavemap_ros/CHANGELOG.rst b/ros/wavemap_ros/CHANGELOG.rst
index 4988a5c1a..bd0b3dee3 100644
--- a/ros/wavemap_ros/CHANGELOG.rst
+++ b/ros/wavemap_ros/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package wavemap_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+* Annotate code for profiling with Tracy Profiler
+* Improve error messages when reading/writing a file fails
+* Contributors: Victor Reijgwart
+
 1.4.0 (2023-08-30)
 ------------------
 * Document how to configure wavemap
diff --git a/ros/wavemap_ros/app/rosbag_processor.cc b/ros/wavemap_ros/app/rosbag_processor.cc
index ea865af92..7f1b7b5f8 100644
--- a/ros/wavemap_ros/app/rosbag_processor.cc
+++ b/ros/wavemap_ros/app/rosbag_processor.cc
@@ -78,7 +78,7 @@ int main(int argc, char** argv) {
     return -1;
   }
 
-  wavemap_server.getMap()->pruneDistant();
+  wavemap_server.getMap()->prune();
   wavemap_server.publishMap();
 
   if (nh_private.param("keep_alive", false)) {
diff --git a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h
index 8e5dbea9d..fa58e1d1b 100644
--- a/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h
+++ b/ros/wavemap_ros/include/wavemap_ros/impl/wavemap_server_inl.h
@@ -5,6 +5,7 @@
 #include <map>
 #include <unordered_set>
 
+#include <tracy/Tracy.hpp>
 #include <wavemap_msgs/Map.h>
 #include <wavemap_ros_conversions/map_msg_conversions.h>
 
@@ -66,7 +67,10 @@ void WavemapServer::publishHashedMap(HashedMapT* hashed_map,
   convert::mapToRosMsg(*hashed_map,
                        map_msg.hashed_wavelet_octree.emplace_back(),
                        blocks_to_publish);
-  map_pub_.publish(map_msg);
+  {
+    ZoneScopedN("publishMapRosMsg");
+    map_pub_.publish(map_msg);
+  }
 
   // Remove the published blocks from the publication queue
   std::for_each(
diff --git a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h
index d1ff3327b..ab996a00b 100644
--- a/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h
+++ b/ros/wavemap_ros/include/wavemap_ros/wavemap_server.h
@@ -1,6 +1,7 @@
 #ifndef WAVEMAP_ROS_WAVEMAP_SERVER_H_
 #define WAVEMAP_ROS_WAVEMAP_SERVER_H_
 
+#include <filesystem>
 #include <memory>
 #include <string>
 #include <unordered_set>
@@ -53,8 +54,8 @@ class WavemapServer {
                 const WavemapServerConfig& config);
 
   void publishMap(bool republish_whole_map = false);
-  bool saveMap(const std::string& file_path) const;
-  bool loadMap(const std::string& file_path);
+  bool saveMap(const std::filesystem::path& file_path) const;
+  bool loadMap(const std::filesystem::path& file_path);
 
   InputHandler* addInput(const param::Value& integrator_params,
                          const ros::NodeHandle& nh, ros::NodeHandle nh_private);
diff --git a/ros/wavemap_ros/package.xml b/ros/wavemap_ros/package.xml
index 8b4e842a8..1f5b7820d 100644
--- a/ros/wavemap_ros/package.xml
+++ b/ros/wavemap_ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_ros</name>
-  <version>1.4.0</version>
+  <version>1.5.0</version>
   <description>ROS interface for wavemap.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
@@ -21,6 +21,7 @@
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>tf2_ros</depend>
+  <depend>tracy_catkin</depend>
 
   <depend>wavemap_msgs</depend>
   <depend>std_srvs</depend>
diff --git a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc
index e04b751d7..134c20eef 100644
--- a/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc
+++ b/ros/wavemap_ros/src/input_handler/depth_image_input_handler.cc
@@ -2,6 +2,7 @@
 
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/core/eigen.hpp>
+#include <tracy/Tracy.hpp>
 #include <wavemap/iterator/grid_iterator.h>
 #include <wavemap/utils/eigen_format.h>
 
@@ -57,6 +58,7 @@ DepthImageInputHandler::DepthImageInputHandler(
 }
 
 void DepthImageInputHandler::processQueue() {
+  ZoneScoped;
   while (!depth_image_queue_.empty()) {
     const sensor_msgs::Image& oldest_msg = depth_image_queue_.front();
     const std::string sensor_frame_id = config_.sensor_frame_id.empty()
@@ -130,6 +132,7 @@ void DepthImageInputHandler::processQueue() {
         }
       }
     }
+    FrameMarkNamed("DepthImage");
 
     // Remove the depth image from the queue
     depth_image_queue_.pop();
@@ -138,6 +141,7 @@ void DepthImageInputHandler::processQueue() {
 
 PosedPointcloud<> DepthImageInputHandler::reproject(
     const PosedImage<>& posed_range_image) {
+  ZoneScoped;
   auto projective_integrator =
       std::dynamic_pointer_cast<ProjectiveIntegrator>(integrators_.front());
   if (!projective_integrator) {
diff --git a/ros/wavemap_ros/src/input_handler/input_handler.cc b/ros/wavemap_ros/src/input_handler/input_handler.cc
index 33a9230a6..8a5c23deb 100644
--- a/ros/wavemap_ros/src/input_handler/input_handler.cc
+++ b/ros/wavemap_ros/src/input_handler/input_handler.cc
@@ -5,6 +5,7 @@
 #include <sensor_msgs/PointCloud.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/point_cloud_conversion.h>
+#include <tracy/Tracy.hpp>
 #include <wavemap/integrator/integrator_factory.h>
 #include <wavemap/integrator/projective/projective_integrator.h>
 
@@ -77,6 +78,7 @@ InputHandler::InputHandler(const InputHandlerConfig& config,
 
 void InputHandler::publishReprojectedPointcloud(
     const ros::Time& stamp, const PosedPointcloud<>& posed_pointcloud) {
+  ZoneScoped;
   sensor_msgs::PointCloud pointcloud_msg;
   pointcloud_msg.header.stamp = stamp;
   pointcloud_msg.header.frame_id = world_frame_;
@@ -96,6 +98,7 @@ void InputHandler::publishReprojectedPointcloud(
 
 void InputHandler::publishProjectedRangeImage(const ros::Time& stamp,
                                               const Image<>& range_image) {
+  ZoneScoped;
   cv_bridge::CvImage cv_image;
   cv_image.header.stamp = stamp;
   cv_image.encoding = "32FC1";
diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc
index 81c324e3f..3a12e7254 100644
--- a/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc
+++ b/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc
@@ -1,6 +1,7 @@
 #include "wavemap_ros/input_handler/pointcloud_input_handler.h"
 
 #include <sensor_msgs/point_cloud2_iterator.h>
+#include <tracy/Tracy.hpp>
 #include <wavemap/integrator/projective/projective_integrator.h>
 #include <wavemap_ros_conversions/time_conversions.h>
 
@@ -49,6 +50,7 @@ PointcloudInputHandler::PointcloudInputHandler(
 
 void PointcloudInputHandler::callback(
     const sensor_msgs::PointCloud2& pointcloud_msg) {
+  ZoneScoped;
   // Skip empty clouds
   const size_t num_points = pointcloud_msg.height * pointcloud_msg.width;
   if (num_points == 0) {
@@ -124,6 +126,7 @@ void PointcloudInputHandler::callback(
 #ifdef LIVOX_AVAILABLE
 void PointcloudInputHandler::callback(
     const livox_ros_driver2::CustomMsg& pointcloud_msg) {
+  ZoneScoped;
   // Skip empty clouds
   if (pointcloud_msg.points.empty()) {
     ROS_WARN_STREAM("Skipping empty pointcloud with timestamp "
@@ -150,6 +153,7 @@ void PointcloudInputHandler::callback(
 #endif
 
 void PointcloudInputHandler::processQueue() {
+  ZoneScoped;
   while (!pointcloud_queue_.empty()) {
     auto& oldest_msg = pointcloud_queue_.front();
 
@@ -255,6 +259,7 @@ void PointcloudInputHandler::processQueue() {
         }
       }
     }
+    FrameMarkNamed("Pointcloud");
 
     // Remove the pointcloud from the queue
     pointcloud_queue_.pop();
diff --git a/ros/wavemap_ros/src/input_handler/pointcloud_undistorter.cc b/ros/wavemap_ros/src/input_handler/pointcloud_undistorter.cc
index 5a046ff4e..8e1557985 100644
--- a/ros/wavemap_ros/src/input_handler/pointcloud_undistorter.cc
+++ b/ros/wavemap_ros/src/input_handler/pointcloud_undistorter.cc
@@ -1,11 +1,13 @@
 #include "wavemap_ros/input_handler/pointcloud_undistorter.h"
 
+#include <tracy/Tracy.hpp>
 #include <wavemap_ros_conversions/time_conversions.h>
 
 namespace wavemap {
 PointcloudUndistorter::Result PointcloudUndistorter::undistortPointcloud(
     GenericStampedPointcloud& stamped_pointcloud,
     PosedPointcloud<>& undistorted_pointcloud, const std::string& fixed_frame) {
+  ZoneScoped;
   // Get the time interval
   const uint64_t start_time = stamped_pointcloud.getStartTime();
   const uint64_t end_time = stamped_pointcloud.getEndTime();
diff --git a/ros/wavemap_ros/src/rosbag_processor.cc b/ros/wavemap_ros/src/rosbag_processor.cc
index 17bb09b88..6b75b39ff 100644
--- a/ros/wavemap_ros/src/rosbag_processor.cc
+++ b/ros/wavemap_ros/src/rosbag_processor.cc
@@ -1,5 +1,7 @@
 #include "wavemap_ros/rosbag_processor.h"
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap {
 RosbagProcessor::~RosbagProcessor() {
   ROS_INFO("Shutting down...");
@@ -34,6 +36,7 @@ bool RosbagProcessor::bagsContainTopic(const std::string& topic_name) {
 }
 
 bool RosbagProcessor::processAll() {
+  ZoneScoped;
   ros::Time side_tasks_last_timestamp(0);
   const ros::Duration kSideTasksDt(0.01);
 
@@ -51,12 +54,14 @@ bool RosbagProcessor::processAll() {
 
     // Handle callbacks
     if (auto it = callbacks_.find(msg.getTopic()); it != callbacks_.end()) {
+      ZoneScopedN("rosbagMsgHandleCallbacks");
       it->second(msg);
     }
 
     // Handle republishing
     if (auto it = republishers_.find(msg.getTopic());
         it != republishers_.end()) {
+      ZoneScopedN("rosbagMsgPublishToTopic");
       it->second.publish(msg);
     }
 
@@ -73,7 +78,10 @@ bool RosbagProcessor::processAll() {
       }
 
       // Process node callbacks (publishers, timers,...)
-      ros::spinOnce();
+      {
+        ZoneScopedN("rosbagSpinOnce");
+        ros::spinOnce();
+      }
     }
   }
 
diff --git a/ros/wavemap_ros/src/wavemap_server.cc b/ros/wavemap_ros/src/wavemap_server.cc
index ea5dd0cfc..37adb3868 100644
--- a/ros/wavemap_ros/src/wavemap_server.cc
+++ b/ros/wavemap_ros/src/wavemap_server.cc
@@ -1,6 +1,7 @@
 #include "wavemap_ros/wavemap_server.h"
 
 #include <std_srvs/Empty.h>
+#include <tracy/Tracy.hpp>
 #include <wavemap/data_structure/volumetric/volumetric_data_structure_factory.h>
 #include <wavemap/utils/nameof.h>
 #include <wavemap_io/file_conversions.h>
@@ -62,6 +63,7 @@ WavemapServer::WavemapServer(ros::NodeHandle nh, ros::NodeHandle nh_private,
 }
 
 void WavemapServer::publishMap(bool republish_whole_map) {
+  ZoneScoped;
   if (occupancy_map_ && !occupancy_map_->empty()) {
     if (auto* hashed_wavelet_octree =
             dynamic_cast<HashedWaveletOctree*>(occupancy_map_.get());
@@ -83,7 +85,7 @@ void WavemapServer::publishMap(bool republish_whole_map) {
   }
 }
 
-bool WavemapServer::saveMap(const std::string& file_path) const {
+bool WavemapServer::saveMap(const std::filesystem::path& file_path) const {
   if (occupancy_map_) {
     occupancy_map_->threshold();
     return io::mapToFile(*occupancy_map_, file_path);
@@ -93,7 +95,7 @@ bool WavemapServer::saveMap(const std::string& file_path) const {
   return false;
 }
 
-bool WavemapServer::loadMap(const std::string& file_path) {
+bool WavemapServer::loadMap(const std::filesystem::path& file_path) {
   return io::fileToMap(file_path, occupancy_map_);
 }
 
@@ -123,7 +125,7 @@ void WavemapServer::subscribeToTimers(const ros::NodeHandle& nh) {
                     << config_.pruning_period << "s");
     map_pruning_timer_ = nh.createTimer(
         ros::Duration(config_.pruning_period),
-        [this](const auto& /*event*/) { occupancy_map_->prune(); });
+        [this](const auto& /*event*/) { occupancy_map_->pruneDistant(); });
   }
 
   if (0.f < config_.publication_period) {
diff --git a/ros/wavemap_ros_conversions/CHANGELOG.rst b/ros/wavemap_ros_conversions/CHANGELOG.rst
index 773c30ca4..74d2ba37c 100644
--- a/ros/wavemap_ros_conversions/CHANGELOG.rst
+++ b/ros/wavemap_ros_conversions/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package wavemap_ros_conversions
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+* Annotate code for profiling with Tracy Profiler
+* Contributors: Victor Reijgwart
+
 1.4.0 (2023-08-30)
 ------------------
 * Make warnings/errors that can occur when loading configs more descriptive
diff --git a/ros/wavemap_ros_conversions/package.xml b/ros/wavemap_ros_conversions/package.xml
index 19d194057..3a7b8c9f9 100644
--- a/ros/wavemap_ros_conversions/package.xml
+++ b/ros/wavemap_ros_conversions/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_ros_conversions</name>
-  <version>1.4.0</version>
+  <version>1.5.0</version>
   <description>Conversions between wavemap and ROS types.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
@@ -15,6 +15,7 @@
 
   <depend>roscpp</depend>
   <depend>wavemap</depend>
+  <depend>tracy_catkin</depend>
   <depend>wavemap_msgs</depend>
   <depend>eigen_conversions</depend>
 </package>
diff --git a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc
index eb65747f9..7ee798ba5 100644
--- a/ros/wavemap_ros_conversions/src/map_msg_conversions.cc
+++ b/ros/wavemap_ros_conversions/src/map_msg_conversions.cc
@@ -1,5 +1,7 @@
 #include "wavemap_ros_conversions/map_msg_conversions.h"
 
+#include <tracy/Tracy.hpp>
+
 namespace wavemap::convert {
 bool mapToRosMsg(const VolumetricDataStructureBase& map,
                  const std::string& frame_id, const ros::Time& stamp,
@@ -36,6 +38,7 @@ bool mapToRosMsg(const VolumetricDataStructureBase& map,
 
 bool rosMsgToMap(const wavemap_msgs::Map& msg,
                  VolumetricDataStructureBase::Ptr& map) {
+  ZoneScoped;
   // Check validity
   if ((msg.wavelet_octree.size() == 1) !=
       (msg.hashed_wavelet_octree.size() != 1)) {
@@ -68,6 +71,7 @@ bool rosMsgToMap(const wavemap_msgs::Map& msg,
 }
 
 void mapToRosMsg(const WaveletOctree& map, wavemap_msgs::WaveletOctree& msg) {
+  ZoneScoped;
   msg.min_cell_width = map.getMinCellWidth();
   msg.min_log_odds = map.getMinLogOdds();
   msg.max_log_odds = map.getMaxLogOdds();
@@ -90,6 +94,7 @@ void mapToRosMsg(const WaveletOctree& map, wavemap_msgs::WaveletOctree& msg) {
 
 void rosMsgToMap(const wavemap_msgs::WaveletOctree& msg,
                  WaveletOctree::Ptr& map) {
+  ZoneScoped;
   WaveletOctreeConfig config;
   config.min_cell_width = msg.min_cell_width;
   config.min_log_odds = msg.min_log_odds;
@@ -127,6 +132,7 @@ void mapToRosMsg(const HashedWaveletOctree& map,
                  wavemap_msgs::HashedWaveletOctree& msg,
                  const std::optional<std::unordered_set<Index3D, Index3DHash>>&
                      include_blocks) {
+  ZoneScoped;
   struct StackElement {
     const FloatingPoint scale;
     const HashedWaveletOctreeBlock::NodeType& node;
@@ -185,6 +191,7 @@ void mapToRosMsg(const HashedWaveletOctree& map,
 
 void rosMsgToMap(const wavemap_msgs::HashedWaveletOctree& msg,
                  HashedWaveletOctree::Ptr& map) {
+  ZoneScoped;
   HashedWaveletOctreeConfig config;
   config.min_cell_width = msg.min_cell_width;
   config.min_log_odds = msg.min_log_odds;
@@ -234,6 +241,7 @@ void mapToRosMsg(const HashedChunkedWaveletOctree& map,
                  wavemap_msgs::HashedWaveletOctree& msg,
                  const std::optional<std::unordered_set<Index3D, Index3DHash>>&
                      include_blocks) {
+  ZoneScoped;
   struct StackElement {
     const OctreeIndex node_index;
     const HashedChunkedWaveletOctreeBlock::NodeChunkType& chunk;
diff --git a/ros/wavemap_rviz_plugin/CHANGELOG.rst b/ros/wavemap_rviz_plugin/CHANGELOG.rst
index 0403935f9..1ed4b582c 100644
--- a/ros/wavemap_rviz_plugin/CHANGELOG.rst
+++ b/ros/wavemap_rviz_plugin/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_rviz_plugin
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+
 1.4.0 (2023-08-30)
 ------------------
 
diff --git a/ros/wavemap_rviz_plugin/package.xml b/ros/wavemap_rviz_plugin/package.xml
index 5f26d0347..1dbc22161 100644
--- a/ros/wavemap_rviz_plugin/package.xml
+++ b/ros/wavemap_rviz_plugin/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_rviz_plugin</name>
-  <version>1.4.0</version>
+  <version>1.5.0</version>
   <description>Plugin to interactively visualize maps published in wavemap's
     native format.
   </description>
diff --git a/tooling/packages/catkin_setup/CHANGELOG.rst b/tooling/packages/catkin_setup/CHANGELOG.rst
index 8853c839c..580f061a1 100644
--- a/tooling/packages/catkin_setup/CHANGELOG.rst
+++ b/tooling/packages/catkin_setup/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package catkin_setup
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+
 1.4.0 (2023-08-30)
 ------------------
 
diff --git a/tooling/packages/catkin_setup/package.xml b/tooling/packages/catkin_setup/package.xml
index 22b5ecc28..5e3f55fb7 100644
--- a/tooling/packages/catkin_setup/package.xml
+++ b/tooling/packages/catkin_setup/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>catkin_setup</name>
-    <version>1.4.0</version>
+    <version>1.5.0</version>
     <description>Dummy package to make it easy to setup the workspace and generate the setup.[sh|bash|zsh] scripts in CI.</description>
 
     <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/tooling/packages/wavemap_all/CHANGELOG.rst b/tooling/packages/wavemap_all/CHANGELOG.rst
index 35b6d002a..23f8e69dd 100644
--- a/tooling/packages/wavemap_all/CHANGELOG.rst
+++ b/tooling/packages/wavemap_all/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_all
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+
 1.4.0 (2023-08-30)
 ------------------
 
diff --git a/tooling/packages/wavemap_all/package.xml b/tooling/packages/wavemap_all/package.xml
index fbc860010..c0092ea2e 100644
--- a/tooling/packages/wavemap_all/package.xml
+++ b/tooling/packages/wavemap_all/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>wavemap_all</name>
-  <version>1.4.0</version>
+  <version>1.5.0</version>
   <description>Metapackage that builds all wavemap packages.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/tooling/packages/wavemap_utils/CHANGELOG.rst b/tooling/packages/wavemap_utils/CHANGELOG.rst
index 2301b781f..7e8a6da54 100644
--- a/tooling/packages/wavemap_utils/CHANGELOG.rst
+++ b/tooling/packages/wavemap_utils/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_utils
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.5.0 (2023-09-05)
+------------------
+
 1.4.0 (2023-08-30)
 ------------------
 
diff --git a/tooling/packages/wavemap_utils/package.xml b/tooling/packages/wavemap_utils/package.xml
index 3fe1e95b5..63fef660a 100644
--- a/tooling/packages/wavemap_utils/package.xml
+++ b/tooling/packages/wavemap_utils/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>wavemap_utils</name>
-    <version>1.4.0</version>
+    <version>1.5.0</version>
     <description>Small package containing scripts to simplify common wavemap development tasks.</description>
 
     <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/tooling/vcstool/wavemap_https.yml b/tooling/vcstool/wavemap_https.yml
index ae0b50db3..70132a994 100644
--- a/tooling/vcstool/wavemap_https.yml
+++ b/tooling/vcstool/wavemap_https.yml
@@ -19,6 +19,10 @@ repositories:
     type: git
     url: https://github.com/ethz-asl/glog_catkin.git
     version: master
+  tracy_catkin:
+    type: git
+    url: https://github.com/ethz-asl/tracy_catkin.git
+    version: main
   minkindr:
     type: git
     url: https://github.com/ethz-asl/minkindr.git
diff --git a/tooling/vcstool/wavemap_ssh.yml b/tooling/vcstool/wavemap_ssh.yml
index cf375cd95..8fc124be7 100644
--- a/tooling/vcstool/wavemap_ssh.yml
+++ b/tooling/vcstool/wavemap_ssh.yml
@@ -19,6 +19,10 @@ repositories:
     type: git
     url: git@github.com:ethz-asl/glog_catkin.git
     version: master
+  tracy_catkin:
+    type: git
+    url: git@github.com:ethz-asl/tracy_catkin.git
+    version: main
   minkindr:
     type: git
     url: git@github.com:ethz-asl/minkindr.git