From f3db2f84db480cee122fd417ccb8a343ce8a95f3 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 8 Aug 2024 16:00:05 +0200
Subject: [PATCH 01/85] Initial commit for wavemap's Python API

---
 CMakeLists.txt                | 12 ++++++++--
 library/python/CMakeLists.txt | 35 +++++++++++++++++++++++++++
 library/python/pywavemap.cc   | 45 +++++++++++++++++++++++++++++++++++
 3 files changed, 90 insertions(+), 2 deletions(-)
 create mode 100644 library/python/CMakeLists.txt
 create mode 100644 library/python/pywavemap.cc

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0a131f7a4..46ad0be2a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,13 +18,16 @@ if ("$ENV{ROS_VERSION}" STREQUAL "1")
   include_directories(${CATKIN_WS_DEVEL_PATH}/include)
 
   # ROS interfaces and tooling
-  # NOTE: Wavemap's C++ library gets included through interfaces/ros1/wavemap.
   add_subdirectory(interfaces/ros1/wavemap)
   add_subdirectory(interfaces/ros1/wavemap_msgs)
   add_subdirectory(interfaces/ros1/wavemap_ros_conversions)
   add_subdirectory(interfaces/ros1/wavemap_ros)
   add_subdirectory(interfaces/ros1/wavemap_rviz_plugin)
 
+  # Libraries
+  # NOTE: Wavemap's C++ lib is already included through interfaces/ros1/wavemap.
+  add_subdirectory(library/python)
+
   # Usage examples
   add_subdirectory(examples/cpp)
   add_subdirectory(examples/ros1)
@@ -36,8 +39,13 @@ elseif ("$ENV{ROS_VERSION}" STREQUAL "2")
 
 else ()
   # Load in pure CMake mode
-  # In this mode, introspection is available only for the C++ library
+  # In this mode, introspection is available only for the C++ and python libs
+
+  # Libraries
   add_subdirectory(library/cpp)
+  add_subdirectory(library/python)
+
+  # Usage examples
   add_subdirectory(examples/cpp)
 
 endif ()
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
new file mode 100644
index 000000000..02382334b
--- /dev/null
+++ b/library/python/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.15...3.27)
+project(pywavemap)
+
+# Build in Release mode by default
+if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE)
+  set_property(CACHE CMAKE_BUILD_TYPE
+      PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
+endif ()
+
+# Load the wavemap library (if not already loaded)
+if (NOT TARGET wavemap::wavemap_core)
+  add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library/cpp
+      ${CMAKE_CURRENT_BINARY_DIR}/wavemap)
+endif ()
+
+# Load Python
+if (CMAKE_VERSION VERSION_LESS 3.18)
+  set(DEV_MODULE Development)
+else ()
+  set(DEV_MODULE Development.Module)
+endif ()
+find_package(Python 3.8 COMPONENTS Interpreter ${DEV_MODULE} REQUIRED)
+
+# Detect the installed nanobind package and import it into CMake
+execute_process(COMMAND "${Python_EXECUTABLE}" -m nanobind --cmake_dir
+    OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE NB_DIR)
+list(APPEND CMAKE_PREFIX_PATH "${NB_DIR}")
+find_package(nanobind CONFIG REQUIRED)
+
+# Generate the bindings
+nanobind_add_module(pywavemap pywavemap.cc)
+set_wavemap_target_properties(pywavemap)
+target_link_libraries(pywavemap
+    PUBLIC wavemap::wavemap_core wavemap::wavemap_io)
diff --git a/library/python/pywavemap.cc b/library/python/pywavemap.cc
new file mode 100644
index 000000000..2529daaf0
--- /dev/null
+++ b/library/python/pywavemap.cc
@@ -0,0 +1,45 @@
+#include <nanobind/eigen/dense.h>
+#include <nanobind/nanobind.h>
+#include <nanobind/stl/filesystem.h>
+#include <nanobind/stl/shared_ptr.h>
+#include <wavemap/core/map/map_base.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace wavemap;  // NOLINT
+namespace nb = nanobind;
+using namespace nb::literals;  // NOLINT
+
+NB_MODULE(pywavemap, m) {
+  m.doc() =
+      "A fast, efficient and accurate multi-resolution, multi-sensor 3D "
+      "occupancy mapping framework.";
+
+  nb::class_<MapBase>(m, "Map")
+      .def_prop_ro("empty", &MapBase::empty)
+      .def_prop_ro("size", &MapBase::size)
+      .def("threshold", &MapBase::threshold)
+      .def("prune", &MapBase::prune)
+      .def("pruneSmart", &MapBase::pruneSmart)
+      .def("clear", &MapBase::clear)
+      .def_prop_ro("min_cell_width", &MapBase::getMinCellWidth)
+      .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds)
+      .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds)
+      .def_prop_ro("memory_usage", &MapBase::getMemoryUsage)
+      .def_prop_ro("tree_height", &MapBase::getTreeHeight)
+      .def_prop_ro("min_index", &MapBase::getMinIndex)
+      .def_prop_ro("max_index", &MapBase::getMaxIndex)
+      .def("getCellValue", &MapBase::getCellValue)
+      .def("setCellValue", &MapBase::setCellValue)
+      .def("addToCellValue", &MapBase::addToCellValue);
+
+  m.def(
+      "load_map",
+      [](const std::filesystem::path& file_path) -> std::shared_ptr<MapBase> {
+        wavemap::MapBase::Ptr map;
+        if (wavemap::io::fileToMap(file_path, map)) {
+          return map;
+        }
+        return nullptr;
+      },
+      "file_path"_a, "Load a wavemap map from a .wvmp file.");
+}

From 25fbc2b87b923dbf06ed9618c3751da56e22a45a Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 13 Aug 2024 12:30:59 +0200
Subject: [PATCH 02/85] Improve param::Value documentation

---
 .../cpp/include/wavemap/core/config/param.h    | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/library/cpp/include/wavemap/core/config/param.h b/library/cpp/include/wavemap/core/config/param.h
index c46ea5620..8458b8bf8 100644
--- a/library/cpp/include/wavemap/core/config/param.h
+++ b/library/cpp/include/wavemap/core/config/param.h
@@ -4,6 +4,7 @@
 #include <map>
 #include <optional>
 #include <string>
+#include <utility>
 #include <variant>
 #include <vector>
 
@@ -11,8 +12,14 @@
 #include "wavemap/core/utils/meta/type_utils.h"
 
 namespace wavemap::param {
+// Type alias for param::Map keys
 using Name = std::string;
 
+// Template for a param value that can hold either
+// * one of the primitive types specified through PrimitiveValueTs
+// * an array (list) of param values
+// * a map (dictionary) of param values, indexed by keys of type param::Name
+// NOTE: This recursion allows one param value to hold a tree of parameters.
 template <typename... PrimitiveValueTs>
 class ValueT {
  public:
@@ -24,6 +31,12 @@ class ValueT {
   explicit ValueT(T value) : data_(value) {}
   explicit ValueT(double value) : data_(static_cast<FloatingPoint>(value)) {}
 
+  // Method to change the value's current type, possibly emplacing a new value
+  template <class T, class... Args>
+  T& emplace(Args&&... args) {
+    return data_.template emplace<T>(std::forward<Args>(args)...);
+  }
+
   // Methods to check the Value's current type
   template <typename ValueT>
   bool holds() const {
@@ -67,9 +80,14 @@ class ValueT {
   std::variant<PrimitiveValueTs..., Array, Map> data_;
 };
 
+// Default primitive types that a param value can hold
 using PrimitiveValueTypes =
     meta::TypeList<bool, int, FloatingPoint, std::string>;
+
+// Instantiate a param::Value type that can hold our default primitive types
 using Value = meta::inject_type_list_t<ValueT, PrimitiveValueTypes>;
+
+// Lift value Array and Map types into param:: namespace
 using Array = Value::Array;
 using Map = Value::Map;
 }  // namespace wavemap::param

From efdc5ce5506d3e3a4d8c60ce774edae53ea44ac2 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 13 Aug 2024 12:38:27 +0200
Subject: [PATCH 03/85] Conversions from python types to wavemap params

---
 library/python/CMakeLists.txt                 |  3 +-
 library/python/include/CPPLINT.cfg            |  1 +
 .../include/pywavemap/param_conversions.h     | 15 ++++
 library/python/src/param_conversions.cc       | 68 +++++++++++++++++++
 library/python/{ => src}/pywavemap.cc         | 33 ++++++---
 5 files changed, 108 insertions(+), 12 deletions(-)
 create mode 100644 library/python/include/CPPLINT.cfg
 create mode 100644 library/python/include/pywavemap/param_conversions.h
 create mode 100644 library/python/src/param_conversions.cc
 rename library/python/{ => src}/pywavemap.cc (63%)

diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 02382334b..9c525b667 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -29,7 +29,8 @@ list(APPEND CMAKE_PREFIX_PATH "${NB_DIR}")
 find_package(nanobind CONFIG REQUIRED)
 
 # Generate the bindings
-nanobind_add_module(pywavemap pywavemap.cc)
+nanobind_add_module(pywavemap src/pywavemap.cc src/param_conversions.cc)
 set_wavemap_target_properties(pywavemap)
+target_include_directories(pywavemap PUBLIC include)
 target_link_libraries(pywavemap
     PUBLIC wavemap::wavemap_core wavemap::wavemap_io)
diff --git a/library/python/include/CPPLINT.cfg b/library/python/include/CPPLINT.cfg
new file mode 100644
index 000000000..2fce9d52b
--- /dev/null
+++ b/library/python/include/CPPLINT.cfg
@@ -0,0 +1 @@
+root=.
diff --git a/library/python/include/pywavemap/param_conversions.h b/library/python/include/pywavemap/param_conversions.h
new file mode 100644
index 000000000..7f3655918
--- /dev/null
+++ b/library/python/include/pywavemap/param_conversions.h
@@ -0,0 +1,15 @@
+#ifndef PYWAVEMAP_PARAM_CONVERSIONS_H_
+#define PYWAVEMAP_PARAM_CONVERSIONS_H_
+
+#include <nanobind/nanobind.h>
+#include <wavemap/core/config/param.h>
+
+namespace nb = nanobind;
+
+namespace wavemap::convert {
+param::Map toParamMap(const nb::handle& py_value);
+param::Array toParamArray(const nb::handle& py_value);
+param::Value toParamValue(const nb::handle& py_value);
+}  // namespace wavemap::convert
+
+#endif  // PYWAVEMAP_PARAM_CONVERSIONS_H_
diff --git a/library/python/src/param_conversions.cc b/library/python/src/param_conversions.cc
new file mode 100644
index 000000000..511c7ae77
--- /dev/null
+++ b/library/python/src/param_conversions.cc
@@ -0,0 +1,68 @@
+#include "pywavemap/param_conversions.h"
+
+namespace wavemap::convert {
+param::Map toParamMap(const nb::handle& py_value) {  // NOLINT
+  nb::dict py_dict;
+  if (!nb::try_cast(py_value, py_dict)) {
+    LOG(WARNING) << "Expected python dict, but got "
+                 << nb::repr(py_value).c_str() << ".";
+    return {};
+  }
+
+  param::Map param_map;
+  for (const auto& [py_key, py_dict_value] : py_dict) {
+    nb::str py_key_str;
+    if (nb::try_cast(py_key, py_key_str)) {
+      param_map.emplace(py_key_str.c_str(), toParamValue(py_dict_value));
+    } else {
+      LOG(WARNING) << "Ignoring dict entry. Key not convertible to string for "
+                      "element with key "
+                   << nb::repr(py_key).c_str() << " and value "
+                   << nb::repr(py_dict_value).c_str() << ".";
+    }
+  }
+  return param_map;
+}
+
+param::Array toParamArray(const nb::handle& py_value) {  // NOLINT
+  nb::list py_list;
+  if (!nb::try_cast(py_value, py_list)) {
+    LOG(WARNING) << "Expected python list, but got "
+                 << nb::repr(py_value).c_str() << ".";
+    return {};
+  }
+
+  param::Array array;
+  array.reserve(nb::len(py_list));
+  for (const auto& py_element : py_list) {  // NOLINT
+    array.template emplace_back(toParamValue(py_element));
+  }
+  return array;
+}
+
+param::Value toParamValue(const nb::handle& py_value) {  // NOLINT
+  if (nb::bool_ py_bool; nb::try_cast(py_value, py_bool)) {
+    return param::Value{static_cast<bool>(py_bool)};
+  }
+  if (nb::int_ py_int; nb::try_cast(py_value, py_int)) {
+    return param::Value{static_cast<int>(py_int)};
+  }
+  if (nb::float_ py_float; nb::try_cast(py_value, py_float)) {
+    return param::Value{static_cast<double>(py_float)};
+  }
+  if (nb::str py_str; nb::try_cast(py_value, py_str)) {
+    return param::Value(py_str.c_str());
+  }
+  if (nb::isinstance<nb::list>(py_value)) {
+    return param::Value(toParamArray(py_value));
+  }
+  if (nb::isinstance<nb::dict>(py_value)) {
+    return param::Value(toParamMap(py_value));
+  }
+
+  // On error, return an empty array
+  LOG(ERROR) << "Encountered unsupported type while parsing python param "
+             << nb::repr(py_value).c_str() << ".";
+  return param::Value{param::Array{}};
+}
+}  // namespace wavemap::convert
diff --git a/library/python/pywavemap.cc b/library/python/src/pywavemap.cc
similarity index 63%
rename from library/python/pywavemap.cc
rename to library/python/src/pywavemap.cc
index 2529daaf0..3a1c11169 100644
--- a/library/python/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -1,3 +1,4 @@
+#include <glog/logging.h>
 #include <nanobind/eigen/dense.h>
 #include <nanobind/nanobind.h>
 #include <nanobind/stl/filesystem.h>
@@ -5,6 +6,8 @@
 #include <wavemap/core/map/map_base.h>
 #include <wavemap/io/file_conversions.h>
 
+#include "pywavemap/param_conversions.h"
+
 using namespace wavemap;  // NOLINT
 namespace nb = nanobind;
 using namespace nb::literals;  // NOLINT
@@ -14,6 +17,11 @@ NB_MODULE(pywavemap, m) {
       "A fast, efficient and accurate multi-resolution, multi-sensor 3D "
       "occupancy mapping framework.";
 
+  google::InitGoogleLogging("pywavemap");
+  google::InstallFailureSignalHandler();
+  FLAGS_alsologtostderr = true;
+  FLAGS_colorlogtostderr = true;
+
   nb::class_<MapBase>(m, "Map")
       .def_prop_ro("empty", &MapBase::empty)
       .def_prop_ro("size", &MapBase::size)
@@ -30,16 +38,19 @@ NB_MODULE(pywavemap, m) {
       .def_prop_ro("max_index", &MapBase::getMaxIndex)
       .def("getCellValue", &MapBase::getCellValue)
       .def("setCellValue", &MapBase::setCellValue)
-      .def("addToCellValue", &MapBase::addToCellValue);
+      .def("addToCellValue", &MapBase::addToCellValue)
+      .def_static(
+          "load",
+          [](const std::filesystem::path& file_path)
+              -> std::shared_ptr<MapBase> {
+            wavemap::MapBase::Ptr map;
+            if (wavemap::io::fileToMap(file_path, map)) {
+              return map;
+            }
+            return nullptr;
+          },
+          "file_path"_a, "Load a wavemap map from a .wvmp file.");
 
-  m.def(
-      "load_map",
-      [](const std::filesystem::path& file_path) -> std::shared_ptr<MapBase> {
-        wavemap::MapBase::Ptr map;
-        if (wavemap::io::fileToMap(file_path, map)) {
-          return map;
-        }
-        return nullptr;
-      },
-      "file_path"_a, "Load a wavemap map from a .wvmp file.");
+  m.def("parse_params",
+        [](nb::handle params) { convert::toParamValue(params); });
 }

From 55a4f3c1ca03ab6372f51f53856d991d8789053c Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 13 Aug 2024 13:37:29 +0200
Subject: [PATCH 04/85] Support std::optional from syntax when parsing
 std::string

---
 .../core/config/impl/type_selector_inl.h      | 46 +++++++++++--------
 .../wavemap/core/config/type_selector.h       |  3 +-
 2 files changed, 28 insertions(+), 21 deletions(-)

diff --git a/library/cpp/include/wavemap/core/config/impl/type_selector_inl.h b/library/cpp/include/wavemap/core/config/impl/type_selector_inl.h
index e2871dce7..ffd2302f4 100644
--- a/library/cpp/include/wavemap/core/config/impl/type_selector_inl.h
+++ b/library/cpp/include/wavemap/core/config/impl/type_selector_inl.h
@@ -88,20 +88,36 @@ TypeSelector<DerivedTypeSelectorT>::toStr(TypeId type_id) {
   }
 }
 
-template <typename DerivedNamedTypeSetT>
-typename TypeSelector<DerivedNamedTypeSetT>::TypeId
-TypeSelector<DerivedNamedTypeSetT>::toTypeId(const std::string& name) {
-  for (size_t type_idx = 0; type_idx < DerivedNamedTypeSetT::names.size();
+template <typename DerivedTypeSelectorT>
+typename TypeSelector<DerivedTypeSelectorT>::TypeId
+TypeSelector<DerivedTypeSelectorT>::toTypeId(const std::string& name) {
+  for (size_t type_idx = 0; type_idx < DerivedTypeSelectorT::names.size();
        ++type_idx) {
-    if (name == DerivedNamedTypeSetT::names[type_idx]) {
+    if (name == DerivedTypeSelectorT::names[type_idx]) {
       return static_cast<TypeId>(type_idx);
     }
   }
   return kInvalidTypeId;
 }
 
-template <typename DerivedNamedTypeSetT>
-std::optional<DerivedNamedTypeSetT> TypeSelector<DerivedNamedTypeSetT>::from(
+template <typename DerivedTypeSelectorT>
+std::optional<DerivedTypeSelectorT> TypeSelector<DerivedTypeSelectorT>::from(
+    const std::string& type_name) {
+  DerivedTypeSelectorT type_id(type_name);
+  if (!type_id.isValid()) {
+    LOG(WARNING)
+        << "Value of type name param \"" << param::kTypeSelectorKey << "\": \""
+        << type_name
+        << "\" does not match a known type name. Supported type names are ["
+        << print::sequence(DerivedTypeSelectorT::names) << "].";
+    return std::nullopt;
+  }
+
+  return type_id;
+}
+
+template <typename DerivedTypeSelectorT>
+std::optional<DerivedTypeSelectorT> TypeSelector<DerivedTypeSelectorT>::from(
     const param::Value& params) {
   // Read the type name from params
   const auto type_name = param::getTypeStr(params);
@@ -112,21 +128,11 @@ std::optional<DerivedNamedTypeSetT> TypeSelector<DerivedNamedTypeSetT>::from(
   }
 
   // Match the type name to a type id
-  DerivedNamedTypeSetT type_id(type_name.value());
-  if (!type_id.isValid()) {
-    LOG(WARNING)
-        << "Value of type name param \"" << param::kTypeSelectorKey << "\": \""
-        << type_name.value()
-        << "\" does not match a known type name. Supported type names are ["
-        << print::sequence(DerivedNamedTypeSetT::names) << "].";
-    return std::nullopt;
-  }
-
-  return type_id;
+  return from(type_name.value());
 }
 
-template <typename DerivedNamedTypeSetT>
-std::optional<DerivedNamedTypeSetT> TypeSelector<DerivedNamedTypeSetT>::from(
+template <typename DerivedTypeSelectorT>
+std::optional<DerivedTypeSelectorT> TypeSelector<DerivedTypeSelectorT>::from(
     const param::Value& params, const std::string& subconfig_name) {
   if (const auto subconfig_params = params.getChild(subconfig_name);
       subconfig_params) {
diff --git a/library/cpp/include/wavemap/core/config/type_selector.h b/library/cpp/include/wavemap/core/config/type_selector.h
index 0a6ad7561..e843e7350 100644
--- a/library/cpp/include/wavemap/core/config/type_selector.h
+++ b/library/cpp/include/wavemap/core/config/type_selector.h
@@ -38,7 +38,8 @@ struct TypeSelector {
   constexpr TypeId toTypeId() const { return id_; }
   static TypeId toTypeId(const std::string& name);
 
-  // Convenience method to read the type from params
+  // Convenience method to read the type from strings or params
+  static std::optional<DerivedTypeSelectorT> from(const std::string& type_name);
   static std::optional<DerivedTypeSelectorT> from(const param::Value& params);
   static std::optional<DerivedTypeSelectorT> from(
       const param::Value& params, const std::string& subconfig_name);

From 6169267718188cd49fcdcb01dda8f31a8cb23f07 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 14 Aug 2024 13:41:23 +0200
Subject: [PATCH 05/85] Add bindings for Pipeline and make logging configurable

---
 .pre-commit-config.yaml                       |  3 +-
 .../cpp/include/wavemap/pipeline/pipeline.h   |  4 ++
 library/python/CMakeLists.txt                 | 11 +++-
 library/python/include/pywavemap/logging.h    | 12 ++++
 library/python/include/pywavemap/map.h        | 13 ++++
 .../{param_conversions.h => param.h}          | 14 +++--
 library/python/include/pywavemap/pipeline.h   | 13 ++++
 library/python/src/logging.cc                 | 28 +++++++++
 library/python/src/map.cc                     | 46 +++++++++++++++
 .../src/{param_conversions.cc => param.cc}    | 18 ++++--
 library/python/src/pipeline.cc                | 29 +++++++++
 library/python/src/pywavemap.cc               | 59 ++++++-------------
 12 files changed, 195 insertions(+), 55 deletions(-)
 create mode 100644 library/python/include/pywavemap/logging.h
 create mode 100644 library/python/include/pywavemap/map.h
 rename library/python/include/pywavemap/{param_conversions.h => param.h} (54%)
 create mode 100644 library/python/include/pywavemap/pipeline.h
 create mode 100644 library/python/src/logging.cc
 create mode 100644 library/python/src/map.cc
 rename library/python/src/{param_conversions.cc => param.cc} (81%)
 create mode 100644 library/python/src/pipeline.cc

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index a87c21e1e..cf90d7beb 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -67,7 +67,8 @@ repos:
                 --library=googletest, --library=tooling/cppcheck/gazebo,
                 "--enable=warning,performance,portability",
                 "--suppress=constStatement",
-                "--suppress=syntaxError:*test/*/test_*.cc" ]
+                "--suppress=syntaxError:*test/*/test_*.cc",
+                "--suppress=assignBoolToPointer:library/python/*"]
 
   - repo: https://github.com/cheshirekow/cmake-format-precommit
     rev: v0.6.13
diff --git a/library/cpp/include/wavemap/pipeline/pipeline.h b/library/cpp/include/wavemap/pipeline/pipeline.h
index f1588e49a..cbee0ef5c 100644
--- a/library/cpp/include/wavemap/pipeline/pipeline.h
+++ b/library/cpp/include/wavemap/pipeline/pipeline.h
@@ -26,6 +26,10 @@ class Pipeline {
         thread_pool_(thread_pool ? std::move(thread_pool)
                                  : std::make_shared<ThreadPool>()) {}
 
+  // Indicate that copy or move-construction are not supported
+  Pipeline(const Pipeline&) = delete;
+  Pipeline(Pipeline&&) = delete;
+
   void clear();
 
   bool hasIntegrator(const std::string& integrator_name) const;
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 9c525b667..5c192451c 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -29,8 +29,13 @@ list(APPEND CMAKE_PREFIX_PATH "${NB_DIR}")
 find_package(nanobind CONFIG REQUIRED)
 
 # Generate the bindings
-nanobind_add_module(pywavemap src/pywavemap.cc src/param_conversions.cc)
+nanobind_add_module(pywavemap
+    src/pywavemap.cc
+    src/logging.cc
+    src/map.cc
+    src/param.cc
+    src/pipeline.cc)
 set_wavemap_target_properties(pywavemap)
 target_include_directories(pywavemap PUBLIC include)
-target_link_libraries(pywavemap
-    PUBLIC wavemap::wavemap_core wavemap::wavemap_io)
+target_link_libraries(pywavemap PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io wavemap::wavemap_pipeline)
diff --git a/library/python/include/pywavemap/logging.h b/library/python/include/pywavemap/logging.h
new file mode 100644
index 000000000..a4d424767
--- /dev/null
+++ b/library/python/include/pywavemap/logging.h
@@ -0,0 +1,12 @@
+#ifndef PYWAVEMAP_LOGGING_H_
+#define PYWAVEMAP_LOGGING_H_
+
+#include <nanobind/nanobind.h>
+
+namespace nb = nanobind;
+
+namespace wavemap {
+void add_logging_module(nb::module_& m_logging);
+}  // namespace wavemap
+
+#endif  // PYWAVEMAP_LOGGING_H_
diff --git a/library/python/include/pywavemap/map.h b/library/python/include/pywavemap/map.h
new file mode 100644
index 000000000..e59f25f8e
--- /dev/null
+++ b/library/python/include/pywavemap/map.h
@@ -0,0 +1,13 @@
+#ifndef PYWAVEMAP_MAP_H_
+#define PYWAVEMAP_MAP_H_
+
+#include <nanobind/nanobind.h>
+#include <nanobind/stl/filesystem.h>
+
+namespace nb = nanobind;
+
+namespace wavemap {
+void add_map_bindings(nb::module_& m);
+}  // namespace wavemap
+
+#endif  // PYWAVEMAP_MAP_H_
diff --git a/library/python/include/pywavemap/param_conversions.h b/library/python/include/pywavemap/param.h
similarity index 54%
rename from library/python/include/pywavemap/param_conversions.h
rename to library/python/include/pywavemap/param.h
index 7f3655918..27ff9f56f 100644
--- a/library/python/include/pywavemap/param_conversions.h
+++ b/library/python/include/pywavemap/param.h
@@ -1,15 +1,19 @@
-#ifndef PYWAVEMAP_PARAM_CONVERSIONS_H_
-#define PYWAVEMAP_PARAM_CONVERSIONS_H_
+#ifndef PYWAVEMAP_PARAM_H_
+#define PYWAVEMAP_PARAM_H_
 
 #include <nanobind/nanobind.h>
 #include <wavemap/core/config/param.h>
 
 namespace nb = nanobind;
 
-namespace wavemap::convert {
+namespace wavemap {
+namespace convert {
 param::Map toParamMap(const nb::handle& py_value);
 param::Array toParamArray(const nb::handle& py_value);
 param::Value toParamValue(const nb::handle& py_value);
-}  // namespace wavemap::convert
+}  // namespace convert
 
-#endif  // PYWAVEMAP_PARAM_CONVERSIONS_H_
+void add_param_module(nb::module_& m_param);
+}  // namespace wavemap
+
+#endif  // PYWAVEMAP_PARAM_H_
diff --git a/library/python/include/pywavemap/pipeline.h b/library/python/include/pywavemap/pipeline.h
new file mode 100644
index 000000000..d6e5f80a9
--- /dev/null
+++ b/library/python/include/pywavemap/pipeline.h
@@ -0,0 +1,13 @@
+#ifndef PYWAVEMAP_PIPELINE_H_
+#define PYWAVEMAP_PIPELINE_H_
+
+#include <nanobind/nanobind.h>
+#include <wavemap/pipeline/pipeline.h>
+
+namespace nb = nanobind;
+
+namespace wavemap {
+void add_pipeline_bindings(nb::module_& m);
+}  // namespace wavemap
+
+#endif  // PYWAVEMAP_PIPELINE_H_
diff --git a/library/python/src/logging.cc b/library/python/src/logging.cc
new file mode 100644
index 000000000..c588d91eb
--- /dev/null
+++ b/library/python/src/logging.cc
@@ -0,0 +1,28 @@
+#include "pywavemap/logging.h"
+
+#include <glog/logging.h>
+#include <wavemap/core/utils/logging_level.h>
+
+using namespace nb::literals;  // NOLINT
+
+namespace wavemap {
+void add_logging_module(nb::module_& m_logging) {
+  google::InitGoogleLogging("pywavemap");
+  google::InstallFailureSignalHandler();
+  FLAGS_alsologtostderr = true;
+  FLAGS_colorlogtostderr = true;
+  FLAGS_log_prefix = false;
+  m_logging.def(
+      "set_level",
+      [](const std::string& level) {
+        if (const auto glog_level = LoggingLevel::from(level); glog_level) {
+          glog_level->applyToGlog();
+        }
+      },
+      "level"_a = "info", "Set the module's logging level.");
+  m_logging.def(
+      "enable_prefix", [](bool enable) { FLAGS_log_prefix = enable; },
+      "enable"_a = false,
+      "Whether to prefix log messages with timestamps and line numbers.");
+}
+}  // namespace wavemap
diff --git a/library/python/src/map.cc b/library/python/src/map.cc
new file mode 100644
index 000000000..14a1d975a
--- /dev/null
+++ b/library/python/src/map.cc
@@ -0,0 +1,46 @@
+#include "pywavemap/map.h"
+
+#include <nanobind/eigen/dense.h>
+#include <nanobind/stl/shared_ptr.h>
+#include <wavemap/core/map/map_base.h>
+#include <wavemap/core/map/map_factory.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace nb::literals;  // NOLINT
+
+namespace wavemap {
+void add_map_bindings(nb::module_& m) {
+  nb::class_<MapBase>(m, "Map")
+      .def_prop_ro("empty", &MapBase::empty)
+      .def_prop_ro("size", &MapBase::size)
+      .def("threshold", &MapBase::threshold)
+      .def("prune", &MapBase::prune)
+      .def("pruneSmart", &MapBase::pruneSmart)
+      .def("clear", &MapBase::clear)
+      .def_prop_ro("min_cell_width", &MapBase::getMinCellWidth)
+      .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds)
+      .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds)
+      .def_prop_ro("memory_usage", &MapBase::getMemoryUsage)
+      .def_prop_ro("tree_height", &MapBase::getTreeHeight)
+      .def_prop_ro("min_index", &MapBase::getMinIndex)
+      .def_prop_ro("max_index", &MapBase::getMaxIndex)
+      .def("getCellValue", &MapBase::getCellValue)
+      .def("setCellValue", &MapBase::setCellValue)
+      .def("addToCellValue", &MapBase::addToCellValue)
+      .def_static("create",
+                  [](const param::Value& params) -> std::shared_ptr<MapBase> {
+                    return MapFactory::create(params);
+                  })
+      .def_static(
+          "load",
+          [](const std::filesystem::path& file_path)
+              -> std::shared_ptr<MapBase> {
+            std::shared_ptr<MapBase> map;
+            if (wavemap::io::fileToMap(file_path, map)) {
+              return map;
+            }
+            return nullptr;
+          },
+          "file_path"_a, "Load a wavemap map from a .wvmp file.");
+}
+}  // namespace wavemap
diff --git a/library/python/src/param_conversions.cc b/library/python/src/param.cc
similarity index 81%
rename from library/python/src/param_conversions.cc
rename to library/python/src/param.cc
index 511c7ae77..58a7542b4 100644
--- a/library/python/src/param_conversions.cc
+++ b/library/python/src/param.cc
@@ -1,6 +1,7 @@
-#include "pywavemap/param_conversions.h"
+#include "pywavemap/param.h"
 
-namespace wavemap::convert {
+namespace wavemap {
+namespace convert {
 param::Map toParamMap(const nb::handle& py_value) {  // NOLINT
   nb::dict py_dict;
   if (!nb::try_cast(py_value, py_dict)) {
@@ -51,7 +52,7 @@ param::Value toParamValue(const nb::handle& py_value) {  // NOLINT
     return param::Value{static_cast<double>(py_float)};
   }
   if (nb::str py_str; nb::try_cast(py_value, py_str)) {
-    return param::Value(py_str.c_str());
+    return param::Value{std::string{py_str.c_str()}};
   }
   if (nb::isinstance<nb::list>(py_value)) {
     return param::Value(toParamArray(py_value));
@@ -65,4 +66,13 @@ param::Value toParamValue(const nb::handle& py_value) {  // NOLINT
              << nb::repr(py_value).c_str() << ".";
   return param::Value{param::Array{}};
 }
-}  // namespace wavemap::convert
+}  // namespace convert
+
+void add_param_module(nb::module_& m_param) {
+  nb::class_<param::Value>(m_param, "Value")
+      .def("__init__", [](param::Value* t, nb::handle py_value) {
+        new (t) param::Value{convert::toParamValue(py_value)};
+      });
+  nb::implicitly_convertible<nb::handle, param::Value>();
+}
+}  // namespace wavemap
diff --git a/library/python/src/pipeline.cc b/library/python/src/pipeline.cc
new file mode 100644
index 000000000..89b9146bc
--- /dev/null
+++ b/library/python/src/pipeline.cc
@@ -0,0 +1,29 @@
+#include "pywavemap/pipeline.h"
+
+#include <nanobind/stl/shared_ptr.h>
+
+namespace wavemap {
+void add_pipeline_bindings(nb::module_& m) {
+  nb::class_<Pipeline>(m, "Pipeline")
+      .def(nb::init<std::shared_ptr<MapBase>>())
+      .def("clear", &Pipeline::clear)
+      .def("hasIntegrator", &Pipeline::hasIntegrator)
+      .def("eraseIntegrator", &Pipeline::eraseIntegrator)
+      .def("addIntegrator",
+           [](Pipeline& self, const std::string& integrator_name,
+              const param::Value& params) -> void {
+             self.addIntegrator(integrator_name, params);
+           })
+      .def("clearIntegrators", &Pipeline::clearIntegrators)
+      .def("addOperation",
+           [](Pipeline& self, const param::Value& params) -> void {
+             self.addOperation(params);
+           })
+      .def("clearOperations", &Pipeline::clearOperations)
+      .def("runIntegrators", &Pipeline::runIntegrators<PosedPointcloud<>>)
+      .def("runIntegrators", &Pipeline::runIntegrators<PosedImage<>>)
+      .def("runOperations", &Pipeline::runOperations)
+      .def("runPipeline", &Pipeline::runPipeline<PosedPointcloud<>>)
+      .def("runPipeline", &Pipeline::runPipeline<PosedImage<>>);
+}
+}  // namespace wavemap
diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc
index 3a1c11169..a6e12a63a 100644
--- a/library/python/src/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -1,56 +1,31 @@
-#include <glog/logging.h>
-#include <nanobind/eigen/dense.h>
 #include <nanobind/nanobind.h>
-#include <nanobind/stl/filesystem.h>
-#include <nanobind/stl/shared_ptr.h>
-#include <wavemap/core/map/map_base.h>
-#include <wavemap/io/file_conversions.h>
 
-#include "pywavemap/param_conversions.h"
+#include "pywavemap/logging.h"
+#include "pywavemap/map.h"
+#include "pywavemap/param.h"
+#include "pywavemap/pipeline.h"
 
 using namespace wavemap;  // NOLINT
 namespace nb = nanobind;
-using namespace nb::literals;  // NOLINT
 
 NB_MODULE(pywavemap, m) {
   m.doc() =
       "A fast, efficient and accurate multi-resolution, multi-sensor 3D "
       "occupancy mapping framework.";
 
-  google::InitGoogleLogging("pywavemap");
-  google::InstallFailureSignalHandler();
-  FLAGS_alsologtostderr = true;
-  FLAGS_colorlogtostderr = true;
+  // Setup logging for the C++ Library
+  nb::module_ m_logging =
+      m.def_submodule("logging", "Submodule for pywavemap's logging system.");
+  add_logging_module(m_logging);
 
-  nb::class_<MapBase>(m, "Map")
-      .def_prop_ro("empty", &MapBase::empty)
-      .def_prop_ro("size", &MapBase::size)
-      .def("threshold", &MapBase::threshold)
-      .def("prune", &MapBase::prune)
-      .def("pruneSmart", &MapBase::pruneSmart)
-      .def("clear", &MapBase::clear)
-      .def_prop_ro("min_cell_width", &MapBase::getMinCellWidth)
-      .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds)
-      .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds)
-      .def_prop_ro("memory_usage", &MapBase::getMemoryUsage)
-      .def_prop_ro("tree_height", &MapBase::getTreeHeight)
-      .def_prop_ro("min_index", &MapBase::getMinIndex)
-      .def_prop_ro("max_index", &MapBase::getMaxIndex)
-      .def("getCellValue", &MapBase::getCellValue)
-      .def("setCellValue", &MapBase::setCellValue)
-      .def("addToCellValue", &MapBase::addToCellValue)
-      .def_static(
-          "load",
-          [](const std::filesystem::path& file_path)
-              -> std::shared_ptr<MapBase> {
-            wavemap::MapBase::Ptr map;
-            if (wavemap::io::fileToMap(file_path, map)) {
-              return map;
-            }
-            return nullptr;
-          },
-          "file_path"_a, "Load a wavemap map from a .wvmp file.");
+  // Bindings and implicit conversions for wavemap's config system
+  nb::module_ m_param =
+      m.def_submodule("param", "Submodule for wavemap's config system.");
+  add_param_module(m_param);
 
-  m.def("parse_params",
-        [](nb::handle params) { convert::toParamValue(params); });
+  // Bindings for wavemap maps
+  add_map_bindings(m);
+
+  // Bindings for measurement integration and map update pipelines
+  add_pipeline_bindings(m);
 }

From 9cd758ff641dd2d1a3b96e1ae0c90520e71a33ac Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 14 Aug 2024 15:55:54 +0200
Subject: [PATCH 06/85] Make pywavemap pip installable

---
 library/python/CMakeLists.txt            | 71 +++++++++++++++++-------
 library/python/LICENSE                   | 28 ++++++++++
 library/python/README.md                 |  0
 library/python/pyproject.toml            | 32 +++++++++++
 library/python/src/pywavemap/__init__.py |  1 +
 5 files changed, 111 insertions(+), 21 deletions(-)
 create mode 100644 library/python/LICENSE
 create mode 100644 library/python/README.md
 create mode 100644 library/python/pyproject.toml
 create mode 100644 library/python/src/pywavemap/__init__.py

diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 5c192451c..4a7182018 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -1,35 +1,61 @@
 cmake_minimum_required(VERSION 3.15...3.27)
-project(pywavemap)
+project(pywavemap LANGUAGES CXX)
 
-# Build in Release mode by default
-if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
-  set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE)
-  set_property(CACHE CMAKE_BUILD_TYPE
-      PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
+# Warn if the user invokes CMake directly
+if (NOT SKBUILD)
+  message(WARNING "\
+  This CMake file is meant to be executed using 'scikit-build-core'.
+  Running it directly will almost certainly not produce the desired
+  result. If you are a user trying to install this package, use the
+  command below, which will install all necessary build dependencies,
+  compile the package in an isolated environment, and then install it.
+  =====================================================================
+   $ pip install .
+  =====================================================================
+  If you are a software developer, and this is your own package, then
+  it is usually much more efficient to install the build dependencies
+  in your environment once and use the following command that avoids
+  a costly creation of a new virtual environment at every compilation:
+  =====================================================================
+   $ pip install nanobind scikit-build-core[pyproject]
+   $ pip install --no-build-isolation -ve .
+  =====================================================================
+  You may optionally add -Ceditable.rebuild=true to auto-rebuild when
+  the package is imported. Otherwise, you need to rerun the above
+  after editing C++ files.")
 endif ()
 
 # Load the wavemap library (if not already loaded)
 if (NOT TARGET wavemap::wavemap_core)
-  add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library/cpp
-      ${CMAKE_CURRENT_BINARY_DIR}/wavemap)
+  if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../../library/cpp)
+    message(STATUS "Loading wavemap library installed on system")
+    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library/cpp
+        ${CMAKE_CURRENT_BINARY_DIR}/wavemap)
+  else ()
+    set(WAVEMAP_TAG feature/pywavemap)
+    message(STATUS "Loading wavemap library from GitHub (tag ${WAVEMAP_TAG})")
+    cmake_minimum_required(VERSION 3.18)
+    include(FetchContent)
+    FetchContent_Declare(
+        ext_wavemap PREFIX wavemap
+        GIT_REPOSITORY https://github.com/ethz-asl/wavemap.git
+        GIT_TAG ${WAVEMAP_TAG}
+        GIT_SHALLOW 1
+        SOURCE_SUBDIR library/cpp)
+    FetchContent_MakeAvailable(ext_wavemap)
+  endif ()
 endif ()
 
-# Load Python
-if (CMAKE_VERSION VERSION_LESS 3.18)
-  set(DEV_MODULE Development)
-else ()
-  set(DEV_MODULE Development.Module)
-endif ()
-find_package(Python 3.8 COMPONENTS Interpreter ${DEV_MODULE} REQUIRED)
+# Try to import all Python components potentially needed by nanobind
+find_package(Python 3.8
+    REQUIRED COMPONENTS Interpreter Development.Module
+    OPTIONAL_COMPONENTS Development.SABIModule)
 
-# Detect the installed nanobind package and import it into CMake
-execute_process(COMMAND "${Python_EXECUTABLE}" -m nanobind --cmake_dir
-    OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE NB_DIR)
-list(APPEND CMAKE_PREFIX_PATH "${NB_DIR}")
+# Import nanobind through CMake's find_package mechanism
 find_package(nanobind CONFIG REQUIRED)
 
-# Generate the bindings
-nanobind_add_module(pywavemap
+# Compile our extension
+nanobind_add_module(pywavemap STABLE_ABI
     src/pywavemap.cc
     src/logging.cc
     src/map.cc
@@ -39,3 +65,6 @@ set_wavemap_target_properties(pywavemap)
 target_include_directories(pywavemap PUBLIC include)
 target_link_libraries(pywavemap PUBLIC
     wavemap::wavemap_core wavemap::wavemap_io wavemap::wavemap_pipeline)
+
+# Install directive for scikit-build-core
+install(TARGETS pywavemap LIBRARY DESTINATION pywavemap)
diff --git a/library/python/LICENSE b/library/python/LICENSE
new file mode 100644
index 000000000..ac002d42d
--- /dev/null
+++ b/library/python/LICENSE
@@ -0,0 +1,28 @@
+BSD 3-Clause License
+
+Copyright (c) 2023, Autonomous Systems Lab, ETH Zurich
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/library/python/README.md b/library/python/README.md
new file mode 100644
index 000000000..e69de29bb
diff --git a/library/python/pyproject.toml b/library/python/pyproject.toml
new file mode 100644
index 000000000..364447ff7
--- /dev/null
+++ b/library/python/pyproject.toml
@@ -0,0 +1,32 @@
+[build-system]
+requires = ["scikit-build-core >=0.4.3", "nanobind >=1.3.2"]
+build-backend = "scikit_build_core.build"
+
+[project]
+name = "pywavemap"
+version = "2.0.0"
+description = "A fast, efficient and accurate multi-resolution, multi-sensor 3D occupancy mapping framework."
+readme = "README.md"
+requires-python = ">=3.8"
+authors = [
+    { name = "Victor Reijgwart", email = "victorr@ethz.ch" },
+]
+classifiers = ["License :: BSD3"]
+
+[project.urls]
+Homepage = "https://github.com/ethz-asl/wavemap"
+
+[tool.scikit-build]
+build-dir = "build/{wheel_tag}"
+minimum-version = "0.4"
+cmake.minimum-version = "3.18"
+wheel.py-api = "cp312"
+
+[tool.cibuildwheel]
+build-verbosity = 1
+archs = ["auto64"]
+skip = ["cp38-*", "pp38-*"]
+
+[tool.cibuildwheel.macos]
+environment = "MACOSX_DEPLOYMENT_TARGET=10.14"
+archs = ["auto64", "arm64"]
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
new file mode 100644
index 000000000..e49c6cd09
--- /dev/null
+++ b/library/python/src/pywavemap/__init__.py
@@ -0,0 +1 @@
+from .pywavemap import *

From 949e90d9a6cd223b625864ac3c83a20c59f8118d Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 14 Aug 2024 16:36:38 +0200
Subject: [PATCH 07/85] Extend and document Map binding

---
 .../cpp/include/wavemap/core/map/map_base.h   | 21 ++++++
 library/python/src/map.cc                     | 75 +++++++++++++------
 2 files changed, 75 insertions(+), 21 deletions(-)

diff --git a/library/cpp/include/wavemap/core/map/map_base.h b/library/cpp/include/wavemap/core/map/map_base.h
index 6f31b439c..5f9426210 100644
--- a/library/cpp/include/wavemap/core/map/map_base.h
+++ b/library/cpp/include/wavemap/core/map/map_base.h
@@ -63,28 +63,49 @@ class MapBase {
       : config_(config.checkValid()) {}
   virtual ~MapBase() = default;
 
+  // Whether the map is empty
   virtual bool empty() const = 0;
+  // The number of cells or nodes in the map
   virtual size_t size() const = 0;
+  // Threshold the occupancy values of all cells in the map to stay within the
+  // range specified by its min_log_odds and max_log_odds
   virtual void threshold() = 0;
+  // Free up memory by pruning nodes that are no longer needed
+  // NOTE: This pruning operation is lossless and does not alter the estimated
+  //       occupancy posterior.
   virtual void prune() = 0;
+  // Similar to prune(), but avoids de-allocating nodes that were recently
+  // updated and will likely be used again in the near future
   virtual void pruneSmart() {
     // NOTE: This method can be overriden by derived classes to provide more
     //       efficient selective pruning strategies. Otherwise, just prune all.
     prune();
   }
+  // Erase all cells in the map
   virtual void clear() = 0;
 
+  // Maximum map resolution, set as width of smallest cell it can represent
   FloatingPoint getMinCellWidth() const { return config_.min_cell_width; }
+  // Lower threshold for the occupancy values stored in the map, in log-odds
   FloatingPoint getMinLogOdds() const { return config_.min_log_odds; }
+  // Upper threshold for the occupancy values stored in the map, in log-odds
   FloatingPoint getMaxLogOdds() const { return config_.max_log_odds; }
+  // The amount of memory used by the map, in bytes
   virtual size_t getMemoryUsage() const = 0;
+  // Height of the octree used to store the map
+  // NOTE: This value is only defined for multi-resolution maps.
   virtual IndexElement getTreeHeight() const = 0;
 
+  // Index of the minimum corner of the map's Axis Aligned Bounding Box
   virtual Index3D getMinIndex() const = 0;
+  // Index of the maximum corner of the map's Axis Aligned Bounding Box
   virtual Index3D getMaxIndex() const = 0;
 
+  // Query the value of the map at a given index
   virtual FloatingPoint getCellValue(const Index3D& index) const = 0;
+  // Set the value of the map at a given index
   virtual void setCellValue(const Index3D& index, FloatingPoint new_value) = 0;
+  // Increment the value of the map at a given index
   virtual void addToCellValue(const Index3D& index, FloatingPoint update) = 0;
 
   using IndexedLeafVisitorFunction =
diff --git a/library/python/src/map.cc b/library/python/src/map.cc
index 14a1d975a..6ab4f96d9 100644
--- a/library/python/src/map.cc
+++ b/library/python/src/map.cc
@@ -11,26 +11,54 @@ using namespace nb::literals;  // NOLINT
 namespace wavemap {
 void add_map_bindings(nb::module_& m) {
   nb::class_<MapBase>(m, "Map")
-      .def_prop_ro("empty", &MapBase::empty)
-      .def_prop_ro("size", &MapBase::size)
-      .def("threshold", &MapBase::threshold)
-      .def("prune", &MapBase::prune)
-      .def("pruneSmart", &MapBase::pruneSmart)
-      .def("clear", &MapBase::clear)
-      .def_prop_ro("min_cell_width", &MapBase::getMinCellWidth)
-      .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds)
-      .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds)
-      .def_prop_ro("memory_usage", &MapBase::getMemoryUsage)
-      .def_prop_ro("tree_height", &MapBase::getTreeHeight)
-      .def_prop_ro("min_index", &MapBase::getMinIndex)
-      .def_prop_ro("max_index", &MapBase::getMaxIndex)
-      .def("getCellValue", &MapBase::getCellValue)
-      .def("setCellValue", &MapBase::setCellValue)
-      .def("addToCellValue", &MapBase::addToCellValue)
-      .def_static("create",
-                  [](const param::Value& params) -> std::shared_ptr<MapBase> {
-                    return MapFactory::create(params);
-                  })
+      .def_prop_ro("empty", &MapBase::empty, "Whether the map is empty.")
+      .def_prop_ro("size", &MapBase::size,
+                   "The number of cells or nodes in the map, for fixed or "
+                   "multi-resolution maps, respectively.")
+      .def("threshold", &MapBase::threshold,
+           "Threshold the occupancy values of all cells in the map to stay "
+           "within the range specified by its min_log_odds and max_log_odds.")
+      .def("prune", &MapBase::prune,
+           "Free up memory by pruning nodes that are no longer needed. Note "
+           "that this pruning operation is lossless and does not alter the "
+           "estimated occupancy posterior.")
+      .def("pruneSmart", &MapBase::pruneSmart,
+           "Similar to prune(), but avoids de-allocating nodes that were "
+           "recently updated and will likely be used again in the near future.")
+      .def("clear", &MapBase::clear, "Erase all cells in the map.")
+      .def_prop_ro("min_cell_width", &MapBase::getMinCellWidth,
+                   "Maximum map resolution, set as width of smallest cell it "
+                   "can represent.")
+      .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds,
+                   "Lower threshold for the occupancy values stored in the "
+                   "map, in log-odds.")
+      .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds,
+                   "Upper threshold for the occupancy values stored in the "
+                   "map, in log-odds.")
+      .def_prop_ro("memory_usage", &MapBase::getMemoryUsage,
+                   "The amount of memory used by the map, in bytes.")
+      .def_prop_ro("tree_height", &MapBase::getTreeHeight,
+                   "Height of the octree used to store the map. Note that this "
+                   "value is only defined for multi-resolution maps.")
+      .def_prop_ro(
+          "min_index", &MapBase::getMinIndex,
+          "Index of the minimum corner of the map's Axis Aligned Bounding Box.")
+      .def_prop_ro(
+          "max_index", &MapBase::getMaxIndex,
+          "Index of the maximum corner of the map's Axis Aligned Bounding Box.")
+      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+           "Query the value of the map at a given index.")
+      .def("setCellValue", &MapBase::setCellValue, "index"_a,
+           "new_value"_a
+           "Set the value of the map at a given index.")
+      .def("addToCellValue", &MapBase::addToCellValue, "index"_a, "update"_a,
+           "Increment the value of the map at a given index.")
+      .def_static(
+          "create",
+          [](const param::Value& params) -> std::shared_ptr<MapBase> {
+            return MapFactory::create(params);
+          },
+          "parameters"_a, "Create a new map based on the given settings.")
       .def_static(
           "load",
           [](const std::filesystem::path& file_path)
@@ -41,6 +69,11 @@ void add_map_bindings(nb::module_& m) {
             }
             return nullptr;
           },
-          "file_path"_a, "Load a wavemap map from a .wvmp file.");
+          "file_path"_a, "Load a wavemap map from a .wvmp file.")
+      .def(
+          "store",
+          [](const MapBase& self, const std::filesystem::path& file_path)
+              -> bool { return wavemap::io::mapToFile(self, file_path); },
+          "file_path"_a, "Store a wavemap map as a .wvmp file.");
 }
 }  // namespace wavemap

From 54f414073769377fba4b91db37ad3ce6b35089f2 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 14 Aug 2024 18:26:02 +0200
Subject: [PATCH 08/85] Improve consistency between Pointcloud and Image data
 structures

---
 .../wavemap/core/data_structure/image.h       |  7 ++++---
 .../wavemap/core/data_structure/pointcloud.h  | 19 ++++++++++---------
 .../core/utils/iterate/pointcloud_iterator.h  |  9 ++++-----
 .../core/data_structure/test_pointcloud.cc    | 16 +++++++---------
 4 files changed, 25 insertions(+), 26 deletions(-)

diff --git a/library/cpp/include/wavemap/core/data_structure/image.h b/library/cpp/include/wavemap/core/data_structure/image.h
index b75f23fbd..1573b9379 100644
--- a/library/cpp/include/wavemap/core/data_structure/image.h
+++ b/library/cpp/include/wavemap/core/data_structure/image.h
@@ -3,6 +3,7 @@
 
 #include <algorithm>
 #include <memory>
+#include <utility>
 
 #include "wavemap/core/common.h"
 #include "wavemap/core/data_structure/posed_object.h"
@@ -14,6 +15,7 @@ class Image {
  public:
   using Ptr = std::shared_ptr<Image<PixelT>>;
   using ConstPtr = std::shared_ptr<const Image<PixelT>>;
+  using PixelType = PixelT;
   using Data = Eigen::Matrix<PixelT, Eigen::Dynamic, Eigen::Dynamic>;
 
   explicit Image(const Index2D& dimensions,
@@ -23,9 +25,8 @@ class Image {
         PixelT initial_value = data::fill::zero<PixelT>())
       : initial_value_(initial_value),
         data_(Data::Constant(num_rows, num_columns, initial_value)) {}
-  explicit Image(const Data& data,
-                 PixelT initial_value = data::fill::zero<PixelT>())
-      : initial_value_(initial_value), data_(data) {}
+  explicit Image(Data data, PixelT initial_value = data::fill::zero<PixelT>())
+      : initial_value_(initial_value), data_(std::move(data)) {}
 
   bool empty() const { return !size(); }
   size_t size() const { return data_.size(); }
diff --git a/library/cpp/include/wavemap/core/data_structure/pointcloud.h b/library/cpp/include/wavemap/core/data_structure/pointcloud.h
index f32291f57..cb08090cd 100644
--- a/library/cpp/include/wavemap/core/data_structure/pointcloud.h
+++ b/library/cpp/include/wavemap/core/data_structure/pointcloud.h
@@ -2,6 +2,7 @@
 #define WAVEMAP_CORE_DATA_STRUCTURE_POINTCLOUD_H_
 
 #include <limits>
+#include <memory>
 #include <utility>
 
 #include "wavemap/core/common.h"
@@ -13,12 +14,13 @@ template <typename PointT = Point3D>
 class Pointcloud {
  public:
   static constexpr int kDim = dim_v<PointT>;
+  using Ptr = std::shared_ptr<Pointcloud<PointT>>;
+  using ConstPtr = std::shared_ptr<const Pointcloud<PointT>>;
   using PointType = PointT;
-  using PointcloudData = Eigen::Matrix<FloatingPoint, kDim, Eigen::Dynamic>;
+  using Data = Eigen::Matrix<FloatingPoint, kDim, Eigen::Dynamic>;
 
   Pointcloud() = default;
-  explicit Pointcloud(PointcloudData pointcloud)
-      : data_(std::move(pointcloud)) {}
+  explicit Pointcloud(Data pointcloud) : data_(std::move(pointcloud)) {}
 
   template <typename PointContainer>
   explicit Pointcloud(const PointContainer& point_container) {
@@ -37,16 +39,15 @@ class Pointcloud {
   }
   void clear() { data_.resize(kDim, 0); }
 
-  typename PointcloudData::ColXpr operator[](Eigen::Index point_index) {
+  typename Data::ColXpr operator[](Eigen::Index point_index) {
     return data_.col(point_index);
   }
-  typename PointcloudData::ConstColXpr operator[](
-      Eigen::Index point_index) const {
+  typename Data::ConstColXpr operator[](Eigen::Index point_index) const {
     return data_.col(point_index);
   }
 
-  PointcloudData& data() { return data_; }
-  const PointcloudData& data() const { return data_; }
+  Data& data() { return data_; }
+  const Data& data() const { return data_; }
 
   using iterator = PointcloudIterator<Pointcloud, kDim>;
   using const_iterator = PointcloudIterator<const Pointcloud, kDim>;
@@ -58,7 +59,7 @@ class Pointcloud {
   const_iterator cend() const { return const_iterator(*this, data_.cols()); }
 
  private:
-  PointcloudData data_;
+  Data data_;
 };
 
 template <typename PointT = Point3D>
diff --git a/library/cpp/include/wavemap/core/utils/iterate/pointcloud_iterator.h b/library/cpp/include/wavemap/core/utils/iterate/pointcloud_iterator.h
index d7b5136e2..838b611e7 100644
--- a/library/cpp/include/wavemap/core/utils/iterate/pointcloud_iterator.h
+++ b/library/cpp/include/wavemap/core/utils/iterate/pointcloud_iterator.h
@@ -9,14 +9,13 @@ namespace wavemap {
 template <typename PointcloudType, int point_dimensions>
 class PointcloudIterator {
  public:
-  using PointcloudData =
-      Eigen::Matrix<FloatingPoint, point_dimensions, Eigen::Dynamic>;
+  using Data = Eigen::Matrix<FloatingPoint, point_dimensions, Eigen::Dynamic>;
   using difference_type = std::ptrdiff_t;
   using value_type = Point<point_dimensions>;
   using pointer = void;
-  using reference = std::conditional_t<std::is_const_v<PointcloudType>,
-                                       typename PointcloudData::ConstColXpr,
-                                       typename PointcloudData::ColXpr>;
+  using reference =
+      std::conditional_t<std::is_const_v<PointcloudType>,
+                         typename Data::ConstColXpr, typename Data::ColXpr>;
   using iterator_category = std::forward_iterator_tag;
   // NOTE: This iterator does not expose pointers to its values (only
   //       references) since pointers wouldn't play nice with Eigen
diff --git a/library/cpp/test/src/core/data_structure/test_pointcloud.cc b/library/cpp/test/src/core/data_structure/test_pointcloud.cc
index e44d06c98..5108844f9 100644
--- a/library/cpp/test/src/core/data_structure/test_pointcloud.cc
+++ b/library/cpp/test/src/core/data_structure/test_pointcloud.cc
@@ -19,9 +19,8 @@ class PointcloudTest : public FixtureBase, public GeometryGenerator {
       EXPECT_EQ(point, pointcloud[point_idx++]);
     }
   }
-  static void compare(
-      const typename Pointcloud<PointT>::PointcloudData& point_matrix,
-      const Pointcloud<PointT>& pointcloud) {
+  static void compare(const typename Pointcloud<PointT>::Data& point_matrix,
+                      const Pointcloud<PointT>& pointcloud) {
     ASSERT_EQ(point_matrix.size() == 0, pointcloud.empty());
     ASSERT_EQ(point_matrix.cols(), pointcloud.size());
     for (Eigen::Index point_idx = 0; point_idx < point_matrix.cols();
@@ -39,12 +38,11 @@ class PointcloudTest : public FixtureBase, public GeometryGenerator {
     }
   }
 
-  typename Pointcloud<PointT>::PointcloudData getRandomPointMatrix() {
+  typename Pointcloud<PointT>::Data getRandomPointMatrix() {
     constexpr FloatingPoint kMaxCoordinate = 1e3;
     const Eigen::Index random_length = getRandomPointcloudSize();
-    typename Pointcloud<PointT>::PointcloudData random_point_matrix =
-        Pointcloud<PointT>::PointcloudData::Random(dim_v<PointT>,
-                                                   random_length);
+    typename Pointcloud<PointT>::Data random_point_matrix =
+        Pointcloud<PointT>::Data::Random(dim_v<PointT>, random_length);
     random_point_matrix *= kMaxCoordinate;
     return random_point_matrix;
   }
@@ -96,13 +94,13 @@ TYPED_TEST(PointcloudTest, InitializeFromStl) {
 }
 
 TYPED_TEST(PointcloudTest, InitializeFromEigen) {
-  typename Pointcloud<TypeParam>::PointcloudData empty_point_matrix;
+  typename Pointcloud<TypeParam>::Data empty_point_matrix;
   Pointcloud<TypeParam> empty_pointcloud(empty_point_matrix);
   EXPECT_TRUE(empty_pointcloud.empty());
 
   constexpr int kNumRepetitions = 100;
   for (int i = 0; i < kNumRepetitions; ++i) {
-    typename Pointcloud<TypeParam>::PointcloudData random_point_matrix =
+    typename Pointcloud<TypeParam>::Data random_point_matrix =
         TestFixture::getRandomPointMatrix();
     Pointcloud<TypeParam> random_pointcloud(random_point_matrix);
     TestFixture::compare(random_point_matrix, random_pointcloud);

From 51ec8cefae22488c1dcda855f8bab36351cd6d15 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 14 Aug 2024 19:13:57 +0200
Subject: [PATCH 09/85] Tidy up and document bindings further

---
 .../cpp/include/wavemap/core/map/map_base.h   | 45 +++++++------
 .../cpp/include/wavemap/pipeline/pipeline.h   | 27 ++++++--
 library/python/include/pywavemap/map.h        |  1 -
 library/python/include/pywavemap/pipeline.h   |  1 -
 library/python/src/logging.cc                 |  5 +-
 library/python/src/map.cc                     |  3 +-
 library/python/src/param.cc                   |  8 ++-
 library/python/src/pipeline.cc                | 67 +++++++++++++------
 8 files changed, 107 insertions(+), 50 deletions(-)

diff --git a/library/cpp/include/wavemap/core/map/map_base.h b/library/cpp/include/wavemap/core/map/map_base.h
index 5f9426210..fa8611011 100644
--- a/library/cpp/include/wavemap/core/map/map_base.h
+++ b/library/cpp/include/wavemap/core/map/map_base.h
@@ -53,6 +53,9 @@ struct MapBaseConfig : ConfigBase<MapBaseConfig, 3> {
   bool isValid(bool verbose) const override;
 };
 
+/**
+ * Base class for wavemap maps
+ */
 class MapBase {
  public:
   static constexpr int kDim = 3;
@@ -63,49 +66,49 @@ class MapBase {
       : config_(config.checkValid()) {}
   virtual ~MapBase() = default;
 
-  // Whether the map is empty
+  //! Whether the map is empty
   virtual bool empty() const = 0;
-  // The number of cells or nodes in the map
+  //! The number of cells or nodes in the map
   virtual size_t size() const = 0;
-  // Threshold the occupancy values of all cells in the map to stay within the
-  // range specified by its min_log_odds and max_log_odds
+  //! Threshold the occupancy values of all cells in the map to stay within the
+  //! range specified by its min_log_odds and max_log_odds
   virtual void threshold() = 0;
-  // Free up memory by pruning nodes that are no longer needed
-  // NOTE: This pruning operation is lossless and does not alter the estimated
-  //       occupancy posterior.
+  //! Free up memory by pruning nodes that are no longer needed
+  //! @note This pruning operation is lossless and does not alter the estimated
+  //!       occupancy posterior.
   virtual void prune() = 0;
-  // Similar to prune(), but avoids de-allocating nodes that were recently
-  // updated and will likely be used again in the near future
+  //! Similar to prune(), but avoids de-allocating nodes that were recently
+  //! updated and will likely be used again in the near future
   virtual void pruneSmart() {
     // NOTE: This method can be overriden by derived classes to provide more
     //       efficient selective pruning strategies. Otherwise, just prune all.
     prune();
   }
-  // Erase all cells in the map
+  //! Erase all cells in the map
   virtual void clear() = 0;
 
-  // Maximum map resolution, set as width of smallest cell it can represent
+  //! Maximum map resolution, set as width of smallest cell it can represent
   FloatingPoint getMinCellWidth() const { return config_.min_cell_width; }
-  // Lower threshold for the occupancy values stored in the map, in log-odds
+  //! Lower threshold for the occupancy values stored in the map, in log-odds
   FloatingPoint getMinLogOdds() const { return config_.min_log_odds; }
-  // Upper threshold for the occupancy values stored in the map, in log-odds
+  //! Upper threshold for the occupancy values stored in the map, in log-odds
   FloatingPoint getMaxLogOdds() const { return config_.max_log_odds; }
-  // The amount of memory used by the map, in bytes
+  //! The amount of memory used by the map, in bytes
   virtual size_t getMemoryUsage() const = 0;
-  // Height of the octree used to store the map
-  // NOTE: This value is only defined for multi-resolution maps.
+  //! Height of the octree used to store the map
+  //! @note This value is only defined for multi-resolution maps.
   virtual IndexElement getTreeHeight() const = 0;
 
-  // Index of the minimum corner of the map's Axis Aligned Bounding Box
+  //! Index of the minimum corner of the map's Axis Aligned Bounding Box
   virtual Index3D getMinIndex() const = 0;
-  // Index of the maximum corner of the map's Axis Aligned Bounding Box
+  //! Index of the maximum corner of the map's Axis Aligned Bounding Box
   virtual Index3D getMaxIndex() const = 0;
 
-  // Query the value of the map at a given index
+  //! Query the value of the map at a given index
   virtual FloatingPoint getCellValue(const Index3D& index) const = 0;
-  // Set the value of the map at a given index
+  //! Set the value of the map at a given index
   virtual void setCellValue(const Index3D& index, FloatingPoint new_value) = 0;
-  // Increment the value of the map at a given index
+  //! Increment the value of the map at a given index
   virtual void addToCellValue(const Index3D& index, FloatingPoint update) = 0;
 
   using IndexedLeafVisitorFunction =
diff --git a/library/cpp/include/wavemap/pipeline/pipeline.h b/library/cpp/include/wavemap/pipeline/pipeline.h
index cbee0ef5c..03d21201f 100644
--- a/library/cpp/include/wavemap/pipeline/pipeline.h
+++ b/library/cpp/include/wavemap/pipeline/pipeline.h
@@ -14,6 +14,9 @@
 #include "wavemap/pipeline/map_operations/map_operation_factory.h"
 
 namespace wavemap {
+/*
+ * A class to build pipelines of measurement integrators and map operations
+ */
 class Pipeline {
  public:
   using IntegratorMap =
@@ -30,43 +33,59 @@ class Pipeline {
   Pipeline(const Pipeline&) = delete;
   Pipeline(Pipeline&&) = delete;
 
+  //! Deregister all measurement integrators and map operations
   void clear();
 
+  //! Returns true if an integrator with the given name has been registered
   bool hasIntegrator(const std::string& integrator_name) const;
+  //! Deregister the integrator with the given name. Returns true if it existed.
   bool eraseIntegrator(const std::string& integrator_name);
+  //! Create and register a new integrator
   IntegratorBase* addIntegrator(const std::string& integrator_name,
                                 const param::Value& integrator_params);
+  //! Register the given integrator, transferring ownership
   IntegratorBase* addIntegrator(const std::string& integrator_name,
                                 std::unique_ptr<IntegratorBase> integrator);
+  //! Get a pointer to the given integrator, returns nullptr if it does not
+  //! exist
   IntegratorBase* getIntegrator(const std::string& integrator_name);
+  //! Access all registered integrators (read-only)
   const IntegratorMap& getIntegrators() { return integrators_; }
+  //! Deregister all integrators
   void clearIntegrators() { integrators_.clear(); }
 
+  //! Create and register a new map operation
   MapOperationBase* addOperation(const param::Value& operation_params);
+  //! Register the given map operation, transferring ownership
   MapOperationBase* addOperation(std::unique_ptr<MapOperationBase> operation);
+  //! Access all registered map operations (read-only)
   const OperationsArray& getOperations() { return operations_; }
+  //! Deregister all map operations
   void clearOperations() { operations_.clear(); }
 
+  //! Integrate a given measurement
   template <typename MeasurementT>
   bool runIntegrators(const std::vector<std::string>& integrator_names,
                       const MeasurementT& measurement);
+  //! Run the map operations
   void runOperations(bool force_run_all = false);
 
+  //! Integrate a given measurement, then run the map operations
   template <typename MeasurementT>
   bool runPipeline(const std::vector<std::string>& integrator_names,
                    const MeasurementT& measurement);
 
  private:
-  // Map data structure
+  //! Map data structure
   const MapBase::Ptr occupancy_map_;
 
-  // Threadpool shared among all input handlers and operations
+  //! Threadpool shared among all input handlers and operations
   const std::shared_ptr<ThreadPool> thread_pool_;
 
-  // Measurement integrators that update the map
+  //! Measurement integrators that update the map
   IntegratorMap integrators_;
 
-  // Operations to perform after map updates
+  //! Operations to perform after map updates
   OperationsArray operations_;
 };
 }  // namespace wavemap
diff --git a/library/python/include/pywavemap/map.h b/library/python/include/pywavemap/map.h
index e59f25f8e..e6b99d7ab 100644
--- a/library/python/include/pywavemap/map.h
+++ b/library/python/include/pywavemap/map.h
@@ -2,7 +2,6 @@
 #define PYWAVEMAP_MAP_H_
 
 #include <nanobind/nanobind.h>
-#include <nanobind/stl/filesystem.h>
 
 namespace nb = nanobind;
 
diff --git a/library/python/include/pywavemap/pipeline.h b/library/python/include/pywavemap/pipeline.h
index d6e5f80a9..d76889df7 100644
--- a/library/python/include/pywavemap/pipeline.h
+++ b/library/python/include/pywavemap/pipeline.h
@@ -2,7 +2,6 @@
 #define PYWAVEMAP_PIPELINE_H_
 
 #include <nanobind/nanobind.h>
-#include <wavemap/pipeline/pipeline.h>
 
 namespace nb = nanobind;
 
diff --git a/library/python/src/logging.cc b/library/python/src/logging.cc
index c588d91eb..966907772 100644
--- a/library/python/src/logging.cc
+++ b/library/python/src/logging.cc
@@ -7,11 +7,14 @@ using namespace nb::literals;  // NOLINT
 
 namespace wavemap {
 void add_logging_module(nb::module_& m_logging) {
+  // Initialize GLOG
   google::InitGoogleLogging("pywavemap");
   google::InstallFailureSignalHandler();
   FLAGS_alsologtostderr = true;
   FLAGS_colorlogtostderr = true;
   FLAGS_log_prefix = false;
+
+  // Methods to configure GLOG
   m_logging.def(
       "set_level",
       [](const std::string& level) {
@@ -19,7 +22,7 @@ void add_logging_module(nb::module_& m_logging) {
           glog_level->applyToGlog();
         }
       },
-      "level"_a = "info", "Set the module's logging level.");
+      "level"_a = "info", "Set pywavemap's logging level.");
   m_logging.def(
       "enable_prefix", [](bool enable) { FLAGS_log_prefix = enable; },
       "enable"_a = false,
diff --git a/library/python/src/map.cc b/library/python/src/map.cc
index 6ab4f96d9..be5292c8a 100644
--- a/library/python/src/map.cc
+++ b/library/python/src/map.cc
@@ -1,6 +1,7 @@
 #include "pywavemap/map.h"
 
 #include <nanobind/eigen/dense.h>
+#include <nanobind/stl/filesystem.h>
 #include <nanobind/stl/shared_ptr.h>
 #include <wavemap/core/map/map_base.h>
 #include <wavemap/core/map/map_factory.h>
@@ -10,7 +11,7 @@ using namespace nb::literals;  // NOLINT
 
 namespace wavemap {
 void add_map_bindings(nb::module_& m) {
-  nb::class_<MapBase>(m, "Map")
+  nb::class_<MapBase>(m, "Map", "Base class for wavemap maps.")
       .def_prop_ro("empty", &MapBase::empty, "Whether the map is empty.")
       .def_prop_ro("size", &MapBase::size,
                    "The number of cells or nodes in the map, for fixed or "
diff --git a/library/python/src/param.cc b/library/python/src/param.cc
index 58a7542b4..da1002288 100644
--- a/library/python/src/param.cc
+++ b/library/python/src/param.cc
@@ -69,10 +69,16 @@ param::Value toParamValue(const nb::handle& py_value) {  // NOLINT
 }  // namespace convert
 
 void add_param_module(nb::module_& m_param) {
-  nb::class_<param::Value>(m_param, "Value")
+  nb::class_<param::Value>(
+      m_param, "Value",
+      "A class that holds parameter values. Note that one Value can hold a "
+      "primitive type, a list of Values, or a dictionary of Values. One Value "
+      "can therefore hold the information needed to initialize an entire "
+      "config, or even a hierarchy of nested configs.")
       .def("__init__", [](param::Value* t, nb::handle py_value) {
         new (t) param::Value{convert::toParamValue(py_value)};
       });
+
   nb::implicitly_convertible<nb::handle, param::Value>();
 }
 }  // namespace wavemap
diff --git a/library/python/src/pipeline.cc b/library/python/src/pipeline.cc
index 89b9146bc..2219d7ebd 100644
--- a/library/python/src/pipeline.cc
+++ b/library/python/src/pipeline.cc
@@ -1,29 +1,56 @@
 #include "pywavemap/pipeline.h"
 
 #include <nanobind/stl/shared_ptr.h>
+#include <wavemap/pipeline/pipeline.h>
+
+using namespace nb::literals;  // NOLINT
 
 namespace wavemap {
 void add_pipeline_bindings(nb::module_& m) {
-  nb::class_<Pipeline>(m, "Pipeline")
+  nb::class_<Pipeline>(m, "Pipeline",
+                       "A class to build pipelines of measurement integrators "
+                       "and map operations.")
       .def(nb::init<std::shared_ptr<MapBase>>())
-      .def("clear", &Pipeline::clear)
-      .def("hasIntegrator", &Pipeline::hasIntegrator)
-      .def("eraseIntegrator", &Pipeline::eraseIntegrator)
-      .def("addIntegrator",
-           [](Pipeline& self, const std::string& integrator_name,
-              const param::Value& params) -> void {
-             self.addIntegrator(integrator_name, params);
-           })
-      .def("clearIntegrators", &Pipeline::clearIntegrators)
-      .def("addOperation",
-           [](Pipeline& self, const param::Value& params) -> void {
-             self.addOperation(params);
-           })
-      .def("clearOperations", &Pipeline::clearOperations)
-      .def("runIntegrators", &Pipeline::runIntegrators<PosedPointcloud<>>)
-      .def("runIntegrators", &Pipeline::runIntegrators<PosedImage<>>)
-      .def("runOperations", &Pipeline::runOperations)
-      .def("runPipeline", &Pipeline::runPipeline<PosedPointcloud<>>)
-      .def("runPipeline", &Pipeline::runPipeline<PosedImage<>>);
+      .def("clear", &Pipeline::clear,
+           "Deregister all the pipeline's measurement integrators and map "
+           "operations.")
+      .def("hasIntegrator", &Pipeline::hasIntegrator, "integrator_name"_a,
+           "Returns true if an integrator with the given name has been "
+           "registered.")
+      .def("eraseIntegrator", &Pipeline::eraseIntegrator, "integrator_name"_a,
+           "Deregister the integrator with the given name. Returns true if it "
+           "existed.")
+      .def(
+          "addIntegrator",
+          [](Pipeline& self, const std::string& integrator_name,
+             const param::Value& params) -> void {
+            self.addIntegrator(integrator_name, params);
+          },
+          "integrator_name"_a, "integrator_params"_a,
+          "Create and register a new integrator")
+      .def("clearIntegrators", &Pipeline::clearIntegrators,
+           "Deregister all integrators.")
+      .def(
+          "addOperation",
+          [](Pipeline& self, const param::Value& params) -> void {
+            self.addOperation(params);
+          },
+          "operation_params"_a, "Create and register a new map operation.")
+      .def("clearOperations", &Pipeline::clearOperations,
+           "Deregister all map operations")
+      .def("runIntegrators", &Pipeline::runIntegrators<PosedPointcloud<>>,
+           "integrator_names"_a, "posed_pointcloud"_a,
+           "Integrate a given pointcloud.")
+      .def("runIntegrators", &Pipeline::runIntegrators<PosedImage<>>,
+           "integrator_names"_a, "posed_image"_a,
+           "Integrate a given depth image.")
+      .def("runOperations", &Pipeline::runOperations, "force_run_all"_a,
+           "Run the map operations.")
+      .def("runPipeline", &Pipeline::runPipeline<PosedPointcloud<>>,
+           "integrator_names"_a, "posed_pointcloud"_a,
+           "Integrate a given pointcloud, then run the map operations.")
+      .def("runPipeline", &Pipeline::runPipeline<PosedImage<>>,
+           "integrator_names"_a, "posed_image"_a,
+           "Integrate a given depth image, then run the map operations.");
 }
 }  // namespace wavemap

From 05ebde72a5dd56a22b40da6c299c2a278506da79 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 14 Aug 2024 19:14:30 +0200
Subject: [PATCH 10/85] Get CLion introspection to work along scikit builds

---
 library/python/CMakeLists.txt | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 4a7182018..4890c0610 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.15...3.27)
 project(pywavemap LANGUAGES CXX)
 
 # Warn if the user invokes CMake directly
-if (NOT SKBUILD)
+if (NOT SKBUILD AND NOT $ENV{CLION_IDE})
   message(WARNING "\
   This CMake file is meant to be executed using 'scikit-build-core'.
   Running it directly will almost certainly not produce the desired
@@ -52,6 +52,11 @@ find_package(Python 3.8
     OPTIONAL_COMPONENTS Development.SABIModule)
 
 # Import nanobind through CMake's find_package mechanism
+if (NOT SKBUILD AND NOT $ENV{CLION_IDE})
+  execute_process(COMMAND "${Python_EXECUTABLE}" -m nanobind --cmake_dir
+      OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE NB_DIR)
+  list(APPEND CMAKE_PREFIX_PATH "${NB_DIR}")
+endif ()
 find_package(nanobind CONFIG REQUIRED)
 
 # Compile our extension
@@ -59,6 +64,7 @@ nanobind_add_module(pywavemap STABLE_ABI
     src/pywavemap.cc
     src/logging.cc
     src/map.cc
+    src/measurements.cc
     src/param.cc
     src/pipeline.cc)
 set_wavemap_target_properties(pywavemap)

From 6321c052ee7817974fbb73d1b03484f67154a77d Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 14 Aug 2024 19:15:55 +0200
Subject: [PATCH 11/85] Draft bindings for measurement types

---
 .../python/include/pywavemap/measurements.h   | 12 +++++++
 library/python/src/measurements.cc            | 33 +++++++++++++++++++
 library/python/src/pywavemap.cc               |  4 +++
 3 files changed, 49 insertions(+)
 create mode 100644 library/python/include/pywavemap/measurements.h
 create mode 100644 library/python/src/measurements.cc

diff --git a/library/python/include/pywavemap/measurements.h b/library/python/include/pywavemap/measurements.h
new file mode 100644
index 000000000..13ed55beb
--- /dev/null
+++ b/library/python/include/pywavemap/measurements.h
@@ -0,0 +1,12 @@
+#ifndef PYWAVEMAP_MEASUREMENTS_H_
+#define PYWAVEMAP_MEASUREMENTS_H_
+
+#include <nanobind/nanobind.h>
+
+namespace nb = nanobind;
+
+namespace wavemap {
+void add_measurement_bindings(nb::module_& m);
+}
+
+#endif  // PYWAVEMAP_MEASUREMENTS_H_
diff --git a/library/python/src/measurements.cc b/library/python/src/measurements.cc
new file mode 100644
index 000000000..9280042e9
--- /dev/null
+++ b/library/python/src/measurements.cc
@@ -0,0 +1,33 @@
+#include "pywavemap/measurements.h"
+
+#include <nanobind/eigen/dense.h>
+#include <wavemap/core/common.h>
+#include <wavemap/core/data_structure/image.h>
+#include <wavemap/core/data_structure/pointcloud.h>
+
+using namespace nb::literals;  // NOLINT
+
+namespace wavemap {
+void add_measurement_bindings(nb::module_& m) {
+  // Poses
+  nb::class_<Rotation3D>(m, "Rotation")
+      .def(nb::init<Rotation3D::RotationMatrix>(), "rotation_matrix"_a);
+  nb::class_<Transformation3D>(m, "Pose")
+      .def(nb::init<Rotation3D, Vector3D>(), "rotation"_a, "translation"_a)
+      .def(nb::init<Transformation3D::TransformationMatrix>(),
+           "transformation_matrix");
+
+  // Pointclouds
+  nb::class_<Pointcloud<>>(m, "Pointcloud")
+      .def(nb::init<Pointcloud<>::Data>(), "point_matrix"_a);
+  nb::class_<PosedPointcloud<>>(m, "PosedPointcloud")
+      .def(nb::init<Transformation3D, Pointcloud<>>(), "pose"_a,
+           "pointcloud"_a);
+
+  // Images
+  nb::class_<Image<>>(m, "Image")
+      .def(nb::init<Image<>::Data>(), "pixel_matrix"_a);
+  nb::class_<PosedImage<>>(m, "PosedImage")
+      .def(nb::init<Transformation3D, Image<>>(), "pose"_a, "image"_a);
+}
+}  // namespace wavemap
diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc
index a6e12a63a..e6bfd8c2c 100644
--- a/library/python/src/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -2,6 +2,7 @@
 
 #include "pywavemap/logging.h"
 #include "pywavemap/map.h"
+#include "pywavemap/measurements.h"
 #include "pywavemap/param.h"
 #include "pywavemap/pipeline.h"
 
@@ -23,6 +24,9 @@ NB_MODULE(pywavemap, m) {
       m.def_submodule("param", "Submodule for wavemap's config system.");
   add_param_module(m_param);
 
+  // Bindings for measurement types
+  add_measurement_bindings(m);
+
   // Bindings for wavemap maps
   add_map_bindings(m);
 

From 18a4b6286b73f7f9c2f8dc1e6f0b68a2c70fea22 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 15 Aug 2024 10:20:47 +0200
Subject: [PATCH 12/85] Generate stubs for better code completion in IDEs

---
 .gitignore                                  |  5 ++++-
 library/python/CMakeLists.txt               |  6 ++++++
 library/python/cmake/pywavemap-extras.cmake | 10 ++++++++++
 3 files changed, 20 insertions(+), 1 deletion(-)
 create mode 100644 library/python/cmake/pywavemap-extras.cmake

diff --git a/.gitignore b/.gitignore
index 136aa91d0..0a94ca157 100644
--- a/.gitignore
+++ b/.gitignore
@@ -34,10 +34,13 @@ CMakeLists.txt.user
 
 srv/_*.py
 *.pcd
-*.pyc
 qtcreator-*
 *.user
 
+# Python
+*.pyc
+*.pyi
+
 /planning/cfg
 /planning/docs
 /planning/src
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 4890c0610..6a62b1e35 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -74,3 +74,9 @@ target_link_libraries(pywavemap PUBLIC
 
 # Install directive for scikit-build-core
 install(TARGETS pywavemap LIBRARY DESTINATION pywavemap)
+
+# Generate stubs
+include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/pywavemap-extras.cmake)
+pywavemap_add_stub(pywavemap pywavemap)
+pywavemap_add_stub(pywavemap pywavemap.logging)
+pywavemap_add_stub(pywavemap pywavemap.param)
diff --git a/library/python/cmake/pywavemap-extras.cmake b/library/python/cmake/pywavemap-extras.cmake
new file mode 100644
index 000000000..3f3912f15
--- /dev/null
+++ b/library/python/cmake/pywavemap-extras.cmake
@@ -0,0 +1,10 @@
+# Convenience function to generate stubs for a nanobind-based (sub)module
+function(pywavemap_add_stub library_target python_module)
+  string(REPLACE "." "_" module_stub_name "${python_module}_stub")
+  string(REPLACE "." "/" module_subpath ${python_module})
+  nanobind_add_stub(${module_stub_name}
+      MODULE ${python_module}
+      OUTPUT "${CMAKE_SOURCE_DIR}/src/${module_subpath}/__init__.pyi"
+      PYTHON_PATH $<TARGET_FILE_DIR:${library_target}>
+      DEPENDS ${library_target})
+endfunction()

From 5e813823c44eb21ba6ee91b0a21a63670fc81285 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 15 Aug 2024 12:39:38 +0200
Subject: [PATCH 13/85] Setup documentation generation for Python API

---
 docs/conf.py              |  5 ++++-
 docs/latex_index.rst      |  3 ---
 docs/pages/intro.rst      |  7 ++-----
 docs/python_api/index.rst | 25 +++++++++++++++++++++++++
 4 files changed, 31 insertions(+), 9 deletions(-)
 create mode 100644 docs/python_api/index.rst

diff --git a/docs/conf.py b/docs/conf.py
index b3bfcb63c..c61566105 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -18,13 +18,15 @@
 # General configuration
 extensions = [
     'sphinx.ext.mathjax', "sphinx.ext.extlinks", 'sphinx.ext.githubpages',
-    'sphinx_design', 'sphinx_sitemap', 'breathe', 'exhale'
+    'sphinx.ext.autodoc', 'sphinx_design', 'sphinx_sitemap', 'breathe',
+    'exhale'
 ]
 templates_path = ['_templates']
 source_suffix = ['.rst', '.md']
 master_doc = 'index'
 language = 'en'
 exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
+add_module_names = False
 
 # The name of the Pygments (syntax highlighting) style to use
 pygments_style = 'sas'
@@ -110,6 +112,7 @@
     "doxygenStripFromPath": "..",
     # Heavily encouraged optional argument (see docs)
     # "rootFileTitle": "API",
+    "fullApiSubSectionTitle": "C++ API",
     # Suggested optional arguments
     "createTreeView": False,
     # TIP: if using the sphinx-bootstrap-theme, you need
diff --git a/docs/latex_index.rst b/docs/latex_index.rst
index 407c8892a..becf44484 100644
--- a/docs/latex_index.rst
+++ b/docs/latex_index.rst
@@ -12,6 +12,3 @@ Wavemap documentation
    pages/parameters/index
    pages/contributing
    pages/faq
-
-..
-   _TODO: Include the Library API again once more code is documented in Doxygen
diff --git a/docs/pages/intro.rst b/docs/pages/intro.rst
index 158ba2bf9..3e756e9af 100644
--- a/docs/pages/intro.rst
+++ b/docs/pages/intro.rst
@@ -64,12 +64,9 @@ For other citation styles, you can use the `Crosscite's citation formatter <http
      pages/faq
 
     .. toctree::
-     :caption: C++ API
+     :caption: APIs
      :maxdepth: 1
      :hidden:
 
-     cpp_api/class_view_hierarchy
      cpp_api/unabridged_api
-
-..
-   _TODO: Include cpp_api/file_view_hierarchy again once the directory structure is documented in Doxygen
+     python_api/index
diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst
new file mode 100644
index 000000000..bf7d56dc8
--- /dev/null
+++ b/docs/python_api/index.rst
@@ -0,0 +1,25 @@
+Python API
+##########
+.. rstcheck: ignore-directives=automodule
+.. rstcheck: ignore-directives=autoclass
+.. rstcheck: ignore-directives=automethod
+
+pywavemap
+*********
+.. automodule:: pywavemap.pywavemap
+.. autoclass:: pywavemap.Map
+    :members:
+.. autoclass:: pywavemap.Pipeline
+    :members:
+
+logging
+*******
+.. automodule:: pywavemap.pywavemap.logging
+.. automethod:: pywavemap.logging.set_level
+.. automethod:: pywavemap.logging.enable_prefix
+
+param
+*****
+.. automodule:: pywavemap.pywavemap.param
+.. autoclass:: pywavemap.pywavemap.param.Value
+    :members:

From 0eaac3342556ec109fdb460f2a1f34339649f75c Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 15 Aug 2024 16:46:10 +0200
Subject: [PATCH 14/85] Include missing conversion helpers

---
 library/python/src/logging.cc      | 1 +
 library/python/src/measurements.cc | 7 +++++--
 library/python/src/pipeline.cc     | 2 ++
 3 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/library/python/src/logging.cc b/library/python/src/logging.cc
index 966907772..1789986ec 100644
--- a/library/python/src/logging.cc
+++ b/library/python/src/logging.cc
@@ -1,6 +1,7 @@
 #include "pywavemap/logging.h"
 
 #include <glog/logging.h>
+#include <nanobind/stl/string.h>
 #include <wavemap/core/utils/logging_level.h>
 
 using namespace nb::literals;  // NOLINT
diff --git a/library/python/src/measurements.cc b/library/python/src/measurements.cc
index 9280042e9..93913d986 100644
--- a/library/python/src/measurements.cc
+++ b/library/python/src/measurements.cc
@@ -11,11 +11,14 @@ namespace wavemap {
 void add_measurement_bindings(nb::module_& m) {
   // Poses
   nb::class_<Rotation3D>(m, "Rotation")
-      .def(nb::init<Rotation3D::RotationMatrix>(), "rotation_matrix"_a);
+      .def(nb::init<Rotation3D::RotationMatrix>(), "rotation_matrix"_a)
+      .def("inverse", &Rotation3D::inverse, "Compute the rotation's inverse.");
   nb::class_<Transformation3D>(m, "Pose")
       .def(nb::init<Rotation3D, Vector3D>(), "rotation"_a, "translation"_a)
       .def(nb::init<Transformation3D::TransformationMatrix>(),
-           "transformation_matrix");
+           "transformation_matrix")
+      .def("inverse", &Transformation3D::inverse,
+           "Compute the transformation's inverse.");
 
   // Pointclouds
   nb::class_<Pointcloud<>>(m, "Pointcloud")
diff --git a/library/python/src/pipeline.cc b/library/python/src/pipeline.cc
index 2219d7ebd..eeaa02277 100644
--- a/library/python/src/pipeline.cc
+++ b/library/python/src/pipeline.cc
@@ -1,6 +1,8 @@
 #include "pywavemap/pipeline.h"
 
 #include <nanobind/stl/shared_ptr.h>
+#include <nanobind/stl/string.h>
+#include <nanobind/stl/vector.h>
 #include <wavemap/pipeline/pipeline.h>
 
 using namespace nb::literals;  // NOLINT

From 2d2ca39da96e403955ed139fc084e9ed46049faa Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 15 Aug 2024 16:47:28 +0200
Subject: [PATCH 15/85] Explicitly import all members to avoid linting errors
 in user pkgs

---
 library/python/src/pywavemap/__init__.py | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index e49c6cd09..9e41c900a 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -1 +1,7 @@
-from .pywavemap import *
+from .pywavemap import Map
+from .pywavemap import Pipeline
+from .pywavemap import Rotation, Pose
+from .pywavemap import Pointcloud, PosedPointcloud
+from .pywavemap import Image, PosedImage
+from .pywavemap import logging
+from .pywavemap import param

From 087cec37774388c3ba1309f44113df4b37bb85f9 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Fri, 16 Aug 2024 14:26:04 +0200
Subject: [PATCH 16/85] Make bindings private following PEP8 and improve docs

---
 docs/conf.py                                |  1 -
 docs/python_api/index.rst                   | 17 +++++-------
 library/python/CMakeLists.txt               | 29 ++++++++++++++-------
 library/python/cmake/pywavemap-extras.cmake | 10 -------
 library/python/src/pywavemap.cc             | 14 +++++++---
 library/python/src/pywavemap/__init__.py    | 10 +++----
 6 files changed, 41 insertions(+), 40 deletions(-)
 delete mode 100644 library/python/cmake/pywavemap-extras.cmake

diff --git a/docs/conf.py b/docs/conf.py
index c61566105..8c2b2a661 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -26,7 +26,6 @@
 master_doc = 'index'
 language = 'en'
 exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
-add_module_names = False
 
 # The name of the Pygments (syntax highlighting) style to use
 pygments_style = 'sas'
diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst
index bf7d56dc8..6530d7727 100644
--- a/docs/python_api/index.rst
+++ b/docs/python_api/index.rst
@@ -4,22 +4,19 @@ Python API
 .. rstcheck: ignore-directives=autoclass
 .. rstcheck: ignore-directives=automethod
 
-pywavemap
-*********
-.. automodule:: pywavemap.pywavemap
+.. automodule:: pywavemap
+
 .. autoclass:: pywavemap.Map
     :members:
 .. autoclass:: pywavemap.Pipeline
     :members:
 
-logging
-*******
-.. automodule:: pywavemap.pywavemap.logging
+.. automodule:: pywavemap.logging
+
 .. automethod:: pywavemap.logging.set_level
 .. automethod:: pywavemap.logging.enable_prefix
 
-param
-*****
-.. automodule:: pywavemap.pywavemap.param
-.. autoclass:: pywavemap.pywavemap.param.Value
+.. automodule:: pywavemap.param
+
+.. autoclass:: pywavemap.param.Value
     :members:
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 6a62b1e35..c1879b28f 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -60,23 +60,34 @@ endif ()
 find_package(nanobind CONFIG REQUIRED)
 
 # Compile our extension
-nanobind_add_module(pywavemap STABLE_ABI
+nanobind_add_module(_cpp_bindings STABLE_ABI
     src/pywavemap.cc
     src/logging.cc
     src/map.cc
     src/measurements.cc
     src/param.cc
     src/pipeline.cc)
-set_wavemap_target_properties(pywavemap)
-target_include_directories(pywavemap PUBLIC include)
-target_link_libraries(pywavemap PUBLIC
+set_wavemap_target_properties(_cpp_bindings)
+target_include_directories(_cpp_bindings PUBLIC include)
+target_link_libraries(_cpp_bindings PUBLIC
     wavemap::wavemap_core wavemap::wavemap_io wavemap::wavemap_pipeline)
 
 # Install directive for scikit-build-core
-install(TARGETS pywavemap LIBRARY DESTINATION pywavemap)
+install(TARGETS _cpp_bindings LIBRARY DESTINATION pywavemap)
 
 # Generate stubs
-include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/pywavemap-extras.cmake)
-pywavemap_add_stub(pywavemap pywavemap)
-pywavemap_add_stub(pywavemap pywavemap.logging)
-pywavemap_add_stub(pywavemap pywavemap.param)
+nanobind_add_stub(pywavemap_stub
+    MODULE _cpp_bindings
+    OUTPUT "${CMAKE_SOURCE_DIR}/src/pywavemap/__init__.pyi"
+    PYTHON_PATH $<TARGET_FILE_DIR:_cpp_bindings>
+    DEPENDS _cpp_bindings)
+nanobind_add_stub(pywavemap_logging_stub
+    MODULE _cpp_bindings.logging
+    OUTPUT "${CMAKE_SOURCE_DIR}/src/pywavemap/logging.pyi"
+    PYTHON_PATH $<TARGET_FILE_DIR:_cpp_bindings>
+    DEPENDS _cpp_bindings)
+nanobind_add_stub(pywavemap_param_stub
+    MODULE _cpp_bindings.param
+    OUTPUT "${CMAKE_SOURCE_DIR}/src/pywavemap/param.pyi"
+    PYTHON_PATH $<TARGET_FILE_DIR:_cpp_bindings>
+    DEPENDS _cpp_bindings)
diff --git a/library/python/cmake/pywavemap-extras.cmake b/library/python/cmake/pywavemap-extras.cmake
deleted file mode 100644
index 3f3912f15..000000000
--- a/library/python/cmake/pywavemap-extras.cmake
+++ /dev/null
@@ -1,10 +0,0 @@
-# Convenience function to generate stubs for a nanobind-based (sub)module
-function(pywavemap_add_stub library_target python_module)
-  string(REPLACE "." "_" module_stub_name "${python_module}_stub")
-  string(REPLACE "." "/" module_subpath ${python_module})
-  nanobind_add_stub(${module_stub_name}
-      MODULE ${python_module}
-      OUTPUT "${CMAKE_SOURCE_DIR}/src/${module_subpath}/__init__.pyi"
-      PYTHON_PATH $<TARGET_FILE_DIR:${library_target}>
-      DEPENDS ${library_target})
-endfunction()
diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc
index e6bfd8c2c..fb4739ac0 100644
--- a/library/python/src/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -9,19 +9,27 @@
 using namespace wavemap;  // NOLINT
 namespace nb = nanobind;
 
-NB_MODULE(pywavemap, m) {
+NB_MODULE(_cpp_bindings, m) {
   m.doc() =
+      "pywavemap\n"
+      "*********\n"
       "A fast, efficient and accurate multi-resolution, multi-sensor 3D "
       "occupancy mapping framework.";
 
   // Setup logging for the C++ Library
   nb::module_ m_logging =
-      m.def_submodule("logging", "Submodule for pywavemap's logging system.");
+      m.def_submodule("logging",
+                      "logging\n"
+                      "=======\n"
+                      "Submodule to configure wavemap's logging system.");
   add_logging_module(m_logging);
 
   // Bindings and implicit conversions for wavemap's config system
   nb::module_ m_param =
-      m.def_submodule("param", "Submodule for wavemap's config system.");
+      m.def_submodule("param",
+                      "param\n"
+                      "=====\n"
+                      "Submodule for wavemap's config system.");
   add_param_module(m_param);
 
   // Bindings for measurement types
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index 9e41c900a..287ff7a1d 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -1,7 +1,3 @@
-from .pywavemap import Map
-from .pywavemap import Pipeline
-from .pywavemap import Rotation, Pose
-from .pywavemap import Pointcloud, PosedPointcloud
-from .pywavemap import Image, PosedImage
-from .pywavemap import logging
-from .pywavemap import param
+from ._cpp_bindings import __doc__  # Use module doc string as pkg doc string
+from ._cpp_bindings import (Map, Pipeline, Rotation, Pose, Pointcloud,
+                            PosedPointcloud, Image, PosedImage, logging, param)

From d767f7763a7532a9b3e214b67ee7510444e17f59 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Fri, 16 Aug 2024 14:35:16 +0200
Subject: [PATCH 17/85] Example script to build a map using the panoptic
 mapping dataset

---
 examples/python/panoptic_mapping.py | 145 ++++++++++++++++++++++++++++
 1 file changed, 145 insertions(+)
 create mode 100644 examples/python/panoptic_mapping.py

diff --git a/examples/python/panoptic_mapping.py b/examples/python/panoptic_mapping.py
new file mode 100644
index 000000000..9305a3731
--- /dev/null
+++ b/examples/python/panoptic_mapping.py
@@ -0,0 +1,145 @@
+# !/usr/bin/env python3
+
+import os
+import csv
+from PIL import Image as PilImage
+import numpy as np
+import pywavemap as pw
+
+
+class DataLoader():
+    # pylint: disable=R0902
+    def __init__(self, data_path):
+        self.data_path = data_path
+
+        self.map = pw.Map.create({
+            "type": "hashed_chunked_wavelet_octree",
+            "min_cell_width": {
+                "meters": 0.05
+            }
+        })
+
+        self.pipeline = pw.Pipeline(self.map)
+
+        self.pipeline.addOperation({
+            "type": "threshold_map",
+            "once_every": {
+                "seconds": 5.0
+            }
+        })
+        self.pipeline.addOperation({
+            "type": "prune_map",
+            "once_every": {
+                "seconds": 10.0
+            }
+        })
+
+        self.pipeline.addIntegrator(
+            "dummy_integrator", {
+                "projection_model": {
+                    "type": "pinhole_camera_projector",
+                    "width": 640,
+                    "height": 480,
+                    "fx": 320.0,
+                    "fy": 320.0,
+                    "cx": 320.0,
+                    "cy": 240.0
+                },
+                "measurement_model": {
+                    "type": "continuous_ray",
+                    "range_sigma": {
+                        "meters": 0.01
+                    },
+                    "scaling_free": 0.2,
+                    "scaling_occupied": 0.4
+                },
+                "integration_method": {
+                    "type": "hashed_chunked_wavelet_integrator",
+                    "min_range": {
+                        "meters": 0.1
+                    },
+                    "max_range": {
+                        "meters": 5.0
+                    }
+                },
+            })
+
+        # setup
+        stamps_file = os.path.join(self.data_path, 'timestamps.csv')
+        self.times = []
+        self.ids = []
+        self.current_index = 0  # Used to iterate through
+        if not os.path.isfile(stamps_file):
+            print(f"No timestamp file '{stamps_file}' found.")
+        with open(stamps_file, 'r') as read_obj:
+            csv_reader = csv.reader(read_obj)
+            for row in csv_reader:
+                if row[0] == "ImageID":
+                    continue
+                self.ids.append(str(row[0]))
+                self.times.append(float(row[1]) / 1e9)
+
+        self.ids = [x for _, x in sorted(zip(self.times, self.ids))]
+        self.times = sorted(self.times)
+
+    def run(self):
+        while self.integrate_frame():
+            pass
+
+    def integrate_frame(self):
+        # Check we're not done.
+        if self.current_index >= len(self.times):
+            return False
+        print(f"Integrating frame {self.current_index} of {len(self.times)}")
+
+        # Get all data and publish.
+        file_id = os.path.join(self.data_path, self.ids[self.current_index])
+
+        # Read the image and pose
+        depth_file = file_id + "_depth.tiff"
+        pose_file = file_id + "_pose.txt"
+        files = [depth_file, pose_file]
+        for f in files:
+            if not os.path.isfile(f):
+                print(f"Could not find file '{f}', skipping frame.")
+                self.current_index += 1
+                return False
+
+        # Load depth image
+        cv_img = PilImage.open(depth_file)
+        image = pw.Image(np.array(cv_img).transpose())
+
+        # Load transform
+        if os.path.isfile(pose_file):
+            with open(pose_file, 'r') as f:
+                pose_data = [float(x) for x in f.read().split()]
+                transform = np.eye(4)
+                for row in range(4):
+                    for col in range(4):
+                        transform[row, col] = pose_data[row * 4 + col]
+        pose = pw.Pose(transform)
+
+        self.pipeline.runPipeline(["dummy_integrator"],
+                                  pw.PosedImage(pose, image))
+
+        self.current_index += 1
+
+        return True
+
+    def save_map(self, path):
+        print(f"Saving map of size {self.map.memory_usage}")
+        self.map.store(path)
+
+
+if __name__ == '__main__':
+    user_home = os.path.expanduser('~')
+    panoptic_mapping_dir = os.path.join(user_home,
+                                        "data/panoptic_mapping/flat_dataset")
+    panoptic_mapping_seq = "run2"
+    output_map_path = os.path.join(
+        user_home, f"panoptic_mapping_{panoptic_mapping_seq}.wvmp")
+    data_loader = DataLoader(
+        os.path.join(panoptic_mapping_dir, panoptic_mapping_seq))
+    data_loader.run()
+    data_loader.save_map(output_map_path)
+    del data_loader  # To avoid mem leak warnings on older Python versions

From c4e8dc36da6fade836a156c1b2bb4bd015bf7dc9 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 19 Aug 2024 16:07:27 +0200
Subject: [PATCH 18/85] Manually provide func signatures to avoid input
 mismatch warnings in IDE

---
 library/python/CMakeLists.txt      | 2 +-
 library/python/src/map.cc          | 3 ++-
 library/python/src/pipeline.cc     | 3 +++
 tooling/scripts/prepare_release.py | 5 +++--
 4 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index c1879b28f..4596ab7e2 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -52,7 +52,7 @@ find_package(Python 3.8
     OPTIONAL_COMPONENTS Development.SABIModule)
 
 # Import nanobind through CMake's find_package mechanism
-if (NOT SKBUILD AND NOT $ENV{CLION_IDE})
+if (NOT SKBUILD)
   execute_process(COMMAND "${Python_EXECUTABLE}" -m nanobind --cmake_dir
       OUTPUT_STRIP_TRAILING_WHITESPACE OUTPUT_VARIABLE NB_DIR)
   list(APPEND CMAKE_PREFIX_PATH "${NB_DIR}")
diff --git a/library/python/src/map.cc b/library/python/src/map.cc
index be5292c8a..411f48fe5 100644
--- a/library/python/src/map.cc
+++ b/library/python/src/map.cc
@@ -59,7 +59,8 @@ void add_map_bindings(nb::module_& m) {
           [](const param::Value& params) -> std::shared_ptr<MapBase> {
             return MapFactory::create(params);
           },
-          "parameters"_a, "Create a new map based on the given settings.")
+          nb::sig("def create(parameters: dict) -> Map"), "parameters"_a,
+          "Create a new map based on the given settings.")
       .def_static(
           "load",
           [](const std::filesystem::path& file_path)
diff --git a/library/python/src/pipeline.cc b/library/python/src/pipeline.cc
index eeaa02277..4a80f50d6 100644
--- a/library/python/src/pipeline.cc
+++ b/library/python/src/pipeline.cc
@@ -28,6 +28,8 @@ void add_pipeline_bindings(nb::module_& m) {
              const param::Value& params) -> void {
             self.addIntegrator(integrator_name, params);
           },
+          nb::sig("def addIntegrator(self, integrator_name: str, "
+                  "integrator_params: dict) -> None"),
           "integrator_name"_a, "integrator_params"_a,
           "Create and register a new integrator")
       .def("clearIntegrators", &Pipeline::clearIntegrators,
@@ -37,6 +39,7 @@ void add_pipeline_bindings(nb::module_& m) {
           [](Pipeline& self, const param::Value& params) -> void {
             self.addOperation(params);
           },
+          nb::sig("def addOperation(self, operation_params: dict) -> None"),
           "operation_params"_a, "Create and register a new map operation.")
       .def("clearOperations", &Pipeline::clearOperations,
            "Deregister all map operations")
diff --git a/tooling/scripts/prepare_release.py b/tooling/scripts/prepare_release.py
index 812240c0a..2cbc365a0 100755
--- a/tooling/scripts/prepare_release.py
+++ b/tooling/scripts/prepare_release.py
@@ -112,9 +112,10 @@ def draft_release_notes():
     out += "`cd wavemap && git checkout main && git pull`\n"
     out += "  * Rebuild wavemap: `catkin build wavemap_all`\n"
     out += "* Docker\n"
-    out += "  * `docker build --tag=wavemap --build-arg=\"VERSION=v1.6.3\" -"
+    out += "  * `docker build --tag=wavemap_ros1 "
+    out += f"--build-arg=\"VERSION=v{new_version_str}\" -"
     out += "<<< $(curl -s https://raw.githubusercontent.com/ethz-asl/wavemap/"
-    out += "main/tooling/docker/incremental.Dockerfile)`\n"
+    out += "main/tooling/docker/ros1/incremental.Dockerfile)`\n"
     out += "For more info, see the [installation page](https://"
     out += "ethz-asl.github.io/wavemap/pages/installation) in the docs.)"
     out += "\n"

From f6b536c4352274fe9ac748db39e4a04c73e2a0d3 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 19 Aug 2024 16:15:07 +0200
Subject: [PATCH 19/85] Fix link to tutorial/examples documentation

---
 examples/README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/README.md b/examples/README.md
index c33614f99..5613a7520 100644
--- a/examples/README.md
+++ b/examples/README.md
@@ -2,4 +2,4 @@
 
 Welcome! We're glad you're interested in using wavemap.
 
-To get started, we recommend taking a look at the [Usage examples](https://ethz-asl.github.io/wavemap/pages/usage_examples.html) documentation page.
+To get started, we recommend taking a look at the [Tutorials](https://ethz-asl.github.io/wavemap/pages/tutorials) documentation page.

From 25be8d5c8ecaf3a7b40714ae1ae7df1ced981d04 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 19 Aug 2024 16:18:28 +0200
Subject: [PATCH 20/85] Fix link to C++ API documentation

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index e59c68ad8..a88248625 100644
--- a/README.md
+++ b/README.md
@@ -33,7 +33,7 @@ The framework's documentation is hosted on [GitHub Pages](https://ethz-asl.githu
 * [Tutorials](https://ethz-asl.github.io/wavemap/pages/tutorials)
 * [Parameters](https://ethz-asl.github.io/wavemap/pages/parameters)
 * [Contributing](https://ethz-asl.github.io/wavemap/pages/contributing)
-* [Library API](https://ethz-asl.github.io/wavemap/api/unabridged_api)
+* [Library API](https://ethz-asl.github.io/wavemap/cpp_api/unabridged_api)
 * [FAQ](https://ethz-asl.github.io/wavemap/pages/faq)
 
 ## Paper

From d5c8f8d9544effb18f069306dcdfd7f19607d9f4 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 19 Aug 2024 17:10:32 +0200
Subject: [PATCH 21/85] Add installation instructions to documentation and
 improve CMake setup

---
 docs/pages/installation/python.rst | 30 +++++++++++++++++++++++++++++-
 library/cpp/cmake/glog/glog.cmake  |  2 +-
 library/python/CMakeLists.txt      |  3 +++
 3 files changed, 33 insertions(+), 2 deletions(-)

diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index e7cbd7362..cff6e75e0 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -1,3 +1,31 @@
 Python
 ######
-Wavemap's Python API is under active development. We will add it to the documentation soon.
+.. rstcheck: ignore-directives=tab-set-code
+
+We will make pywavemap available through PyPI soon. In the meantime, you can already pip install the package from sources.
+
+Before you start, make sure git, Python 3 and pip are available on your system::
+
+    sudo apt update
+    sudo apt install git python3 python3-pip
+
+Next, clone wavemap's code to your machine. We recommend using `SSH <https://docs.github.com/en/authentication/connecting-to-github-with-ssh>`_. Alternatively, HTTPS can be used without requiring keys to be set up.
+
+.. tab-set-code::
+
+    .. code-block:: SSH
+      :class: no-header
+
+      cd ~/
+      git clone git@github.com:ethz-asl/wavemap.git
+
+    .. code-block:: HTTPS
+      :class: no-header
+
+      cd ~/
+      git clone https://github.com/ethz-asl/wavemap.git
+
+You can then install pywavemap by running::
+
+    cd ~/wavemap/library/python
+    pip3 install .
diff --git a/library/cpp/cmake/glog/glog.cmake b/library/cpp/cmake/glog/glog.cmake
index 0b2accc42..399012ea6 100644
--- a/library/cpp/cmake/glog/glog.cmake
+++ b/library/cpp/cmake/glog/glog.cmake
@@ -1,7 +1,7 @@
 include(FetchContent)
 FetchContent_Declare(glog
     GIT_REPOSITORY https://github.com/google/glog.git
-    GIT_TAG v0.6.0)
+    GIT_TAG v0.4.0)
 FetchContent_GetProperties(glog)
 if (NOT glog_POPULATED)
   FetchContent_Populate(glog)
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 4596ab7e2..b302cf5f6 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -71,6 +71,9 @@ set_wavemap_target_properties(_cpp_bindings)
 target_include_directories(_cpp_bindings PUBLIC include)
 target_link_libraries(_cpp_bindings PUBLIC
     wavemap::wavemap_core wavemap::wavemap_io wavemap::wavemap_pipeline)
+# Disable some default wavemap warnings that trigger on nanobind
+target_compile_options(_cpp_bindings PRIVATE
+    -Wno-pedantic -Wno-suggest-attribute=const)
 
 # Install directive for scikit-build-core
 install(TARGETS _cpp_bindings LIBRARY DESTINATION pywavemap)

From d6a8f71021488953ee242f60882fd3ddf51323f9 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 19 Aug 2024 18:41:09 +0200
Subject: [PATCH 22/85] Debug issues with pip install on other machines

---
 docs/pages/installation/python.rst        | 4 ++--
 library/cpp/cmake/find-wavemap-deps.cmake | 6 +++---
 library/python/CMakeLists.txt             | 4 ++--
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index cff6e75e0..2fb6671ae 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -4,10 +4,10 @@ Python
 
 We will make pywavemap available through PyPI soon. In the meantime, you can already pip install the package from sources.
 
-Before you start, make sure git, Python 3 and pip are available on your system::
+Before you start, make sure you have the necessary dependencies installed to build Python and C++ packages::
 
     sudo apt update
-    sudo apt install git python3 python3-pip
+    sudo apt install git build-essential python3-dev python3-pip
 
 Next, clone wavemap's code to your machine. We recommend using `SSH <https://docs.github.com/en/authentication/connecting-to-github-with-ssh>`_. Alternatively, HTTPS can be used without requiring keys to be set up.
 
diff --git a/library/cpp/cmake/find-wavemap-deps.cmake b/library/cpp/cmake/find-wavemap-deps.cmake
index 6bb85297b..21c551d60 100644
--- a/library/cpp/cmake/find-wavemap-deps.cmake
+++ b/library/cpp/cmake/find-wavemap-deps.cmake
@@ -3,7 +3,7 @@ if (USE_SYSTEM_EIGEN)
   find_package(Eigen3 QUIET NO_MODULE)
 endif ()
 if (USE_SYSTEM_EIGEN AND TARGET Eigen3::Eigen)
-  message(STATUS "Using system Eigen")
+  message(STATUS "Using system Eigen (version ${Eigen3_VERSION})")
 else ()
   message(STATUS "Fetching external Eigen")
   set(USE_SYSTEM_EIGEN OFF)
@@ -21,7 +21,7 @@ if (USE_SYSTEM_GLOG)
   endif ()
 endif ()
 if (USE_SYSTEM_GLOG AND glog_FOUND)
-  message(STATUS "Using system Glog")
+  message(STATUS "Using system Glog (version ${glog_VERSION})")
 else ()
   message(STATUS "Fetching external Glog")
   set(USE_SYSTEM_GLOG OFF)
@@ -42,7 +42,7 @@ if (USE_SYSTEM_BOOST)
   endif ()
 endif ()
 if (USE_SYSTEM_BOOST AND TARGET Boost::preprocessor)
-  message(STATUS "Using system Boost")
+  message(STATUS "Using system Boost (version ${Boost_VERSION})")
 else ()
   message(STATUS "Fetching external Boost")
   set(USE_SYSTEM_BOOST OFF)
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index b302cf5f6..efc5be8b4 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.15...3.27)
+cmake_minimum_required(VERSION 3.18...3.27)
 project(pywavemap LANGUAGES CXX)
 
 # Warn if the user invokes CMake directly
@@ -73,7 +73,7 @@ target_link_libraries(_cpp_bindings PUBLIC
     wavemap::wavemap_core wavemap::wavemap_io wavemap::wavemap_pipeline)
 # Disable some default wavemap warnings that trigger on nanobind
 target_compile_options(_cpp_bindings PRIVATE
-    -Wno-pedantic -Wno-suggest-attribute=const)
+    -Wno-pedantic -Wno-unused-result -Wno-suggest-attribute=const)
 
 # Install directive for scikit-build-core
 install(TARGETS _cpp_bindings LIBRARY DESTINATION pywavemap)

From 9700002bb6daae1c30e5bdb7c5cd2355b1aa1407 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 19 Aug 2024 18:47:03 +0200
Subject: [PATCH 23/85] Set logging level directly instead of using gflags lib

Circumventing gflags helps since it is not always available or linked correctly. An alternative solution would be to automatically make gflags available through FetchContent, but we would rather keep the number of auto-fetched libraries small to reduce compatibility struggles. Furthermore, our library currently disables the option to install if any dependency is auto-fetched.
---
 library/cpp/src/core/utils/logging_level.cc | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/library/cpp/src/core/utils/logging_level.cc b/library/cpp/src/core/utils/logging_level.cc
index 4231ad64b..5e1b18fb6 100644
--- a/library/cpp/src/core/utils/logging_level.cc
+++ b/library/cpp/src/core/utils/logging_level.cc
@@ -3,8 +3,5 @@
 #include <glog/logging.h>
 
 namespace wavemap {
-void LoggingLevel::applyToGlog() const {
-  google::SetCommandLineOption("minloglevel",
-                               std::to_string(toTypeId()).c_str());
-}
+void LoggingLevel::applyToGlog() const { FLAGS_minloglevel = toTypeId(); }
 }  // namespace wavemap

From 35feb1b7eabe551c430ea025ca099ef38db851ed Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 19 Aug 2024 19:17:46 +0200
Subject: [PATCH 24/85] Add dependency on typing_extensions for older python
 versions

---
 library/python/pyproject.toml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/library/python/pyproject.toml b/library/python/pyproject.toml
index 364447ff7..654d0c7ea 100644
--- a/library/python/pyproject.toml
+++ b/library/python/pyproject.toml
@@ -1,5 +1,5 @@
 [build-system]
-requires = ["scikit-build-core >=0.4.3", "nanobind >=1.3.2"]
+requires = ["scikit-build-core >=0.4.3", "nanobind >=1.3.2", "typing_extensions; python_version < '3.11'"]
 build-backend = "scikit_build_core.build"
 
 [project]

From 16feaec4171858db1e80f01d4daeea613c05d81c Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 11:26:58 +0200
Subject: [PATCH 25/85] FetchContent more recent glog version for better CMake
 config support

---
 library/cpp/cmake/find-wavemap-deps.cmake | 4 ++++
 library/cpp/cmake/glog/glog.cmake         | 2 +-
 library/python/CMakeLists.txt             | 2 +-
 3 files changed, 6 insertions(+), 2 deletions(-)

diff --git a/library/cpp/cmake/find-wavemap-deps.cmake b/library/cpp/cmake/find-wavemap-deps.cmake
index 21c551d60..99d6c614b 100644
--- a/library/cpp/cmake/find-wavemap-deps.cmake
+++ b/library/cpp/cmake/find-wavemap-deps.cmake
@@ -1,3 +1,7 @@
+if (CMAKE_VERSION VERSION_GREATER 3.24)
+  cmake_policy(SET CMP0135 OLD)
+endif ()
+
 # Eigen
 if (USE_SYSTEM_EIGEN)
   find_package(Eigen3 QUIET NO_MODULE)
diff --git a/library/cpp/cmake/glog/glog.cmake b/library/cpp/cmake/glog/glog.cmake
index 399012ea6..0b2accc42 100644
--- a/library/cpp/cmake/glog/glog.cmake
+++ b/library/cpp/cmake/glog/glog.cmake
@@ -1,7 +1,7 @@
 include(FetchContent)
 FetchContent_Declare(glog
     GIT_REPOSITORY https://github.com/google/glog.git
-    GIT_TAG v0.4.0)
+    GIT_TAG v0.6.0)
 FetchContent_GetProperties(glog)
 if (NOT glog_POPULATED)
   FetchContent_Populate(glog)
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index efc5be8b4..857beaee9 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.18...3.27)
+cmake_minimum_required(VERSION 3.18)
 project(pywavemap LANGUAGES CXX)
 
 # Warn if the user invokes CMake directly

From 6f4ec7e9b84663f041427278218e07c2bc58ffd2 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 11:49:42 +0200
Subject: [PATCH 26/85] Add Docker files to test pip installs

---
 tooling/docker/python/alpine.Dockerfile | 15 +++++++++++++++
 tooling/docker/python/debian.Dockerfile | 17 +++++++++++++++++
 2 files changed, 32 insertions(+)
 create mode 100644 tooling/docker/python/alpine.Dockerfile
 create mode 100644 tooling/docker/python/debian.Dockerfile

diff --git a/tooling/docker/python/alpine.Dockerfile b/tooling/docker/python/alpine.Dockerfile
new file mode 100644
index 000000000..82ed993ff
--- /dev/null
+++ b/tooling/docker/python/alpine.Dockerfile
@@ -0,0 +1,15 @@
+ARG WAVEMAP_TAG=main
+
+FROM alpine:3.20
+
+ARG WAVEMAP_TAG
+
+# hadolint ignore=DL3018
+RUN apk add --no-cache git build-base python3-dev py3-pip
+
+# hadolint ignore=DL3059
+RUN git clone --branch ${WAVEMAP_TAG} https://github.com/ethz-asl/wavemap.git
+
+WORKDIR /wavemap/library/python
+# hadolint ignore=DL3042
+RUN pip3 install . --break-system-packages
diff --git a/tooling/docker/python/debian.Dockerfile b/tooling/docker/python/debian.Dockerfile
new file mode 100644
index 000000000..f421da4a2
--- /dev/null
+++ b/tooling/docker/python/debian.Dockerfile
@@ -0,0 +1,17 @@
+ARG WAVEMAP_TAG=main
+
+FROM debian:11.10
+
+ARG WAVEMAP_TAG
+
+# hadolint ignore=DL3008
+RUN apt-get update && \
+    apt-get install -q -y --no-install-recommends \
+    git build-essential python3-dev python3-pip && \
+    rm -rf /var/lib/apt/lists/*
+
+RUN git clone --branch ${WAVEMAP_TAG} https://github.com/ethz-asl/wavemap.git
+
+WORKDIR /wavemap/library/python
+# hadolint ignore=DL3042
+RUN pip3 install .

From fe22adfc41204bd5a8a2637897d92604c6d00577 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 11:53:05 +0200
Subject: [PATCH 27/85] Also auto-fetch glog when it is not found by PkgConfig

---
 library/cpp/cmake/find-wavemap-deps.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/library/cpp/cmake/find-wavemap-deps.cmake b/library/cpp/cmake/find-wavemap-deps.cmake
index 99d6c614b..695f0c98e 100644
--- a/library/cpp/cmake/find-wavemap-deps.cmake
+++ b/library/cpp/cmake/find-wavemap-deps.cmake
@@ -20,7 +20,7 @@ if (USE_SYSTEM_GLOG)
   if (NOT glog_FOUND)
     find_package(PkgConfig QUIET)
     if (PkgConfig_FOUND)
-      pkg_check_modules(glog REQUIRED libglog)
+      pkg_check_modules(glog QUIET libglog)
     endif ()
   endif ()
 endif ()

From 67c7e677c1e306247af54880f7f51ef5378cb9be Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 12:35:34 +0200
Subject: [PATCH 28/85] Document how to install pywavemap

---
 docs/pages/installation/python.rst | 52 +++++++++++++++++++++++++++---
 1 file changed, 48 insertions(+), 4 deletions(-)

diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index 2fb6671ae..6811d7933 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -2,12 +2,31 @@ Python
 ######
 .. rstcheck: ignore-directives=tab-set-code
 
-We will make pywavemap available through PyPI soon. In the meantime, you can already pip install the package from sources.
+We will make pywavemap available through PyPI soon. In the meantime, you can already pip install pywavemap from sources.
 
-Before you start, make sure you have the necessary dependencies installed to build Python and C++ packages::
+Regular install
+***************
+In case you'd like to use pywavemap but do not expect to change its code often, we recommend a regular install.
 
-    sudo apt update
-    sudo apt install git build-essential python3-dev python3-pip
+.. _python-install-build-deps:
+
+First, make sure the necessary dependencies to build C++ and Python packages are available on your machine:
+
+.. tab-set-code::
+
+    .. code-block:: Debian/Ubuntu
+      :class: no-header
+
+      sudo apt update
+      sudo apt install git build-essential python3-dev python3-pip
+
+    .. code-block:: Alpine
+      :class: no-header
+
+      apk update
+      apk add git build-base python3-dev py3-pip
+
+.. _python-install-clone-repo:
 
 Next, clone wavemap's code to your machine. We recommend using `SSH <https://docs.github.com/en/authentication/connecting-to-github-with-ssh>`_. Alternatively, HTTPS can be used without requiring keys to be set up.
 
@@ -29,3 +48,28 @@ You can then install pywavemap by running::
 
     cd ~/wavemap/library/python
     pip3 install .
+
+Editable install
+****************
+By default, pip will recompile wavemap's C++ and Python libraries in a virtual environment from scratch on every install. If you're interested in modifying wavemap's code, you can save time by enabling incremental builds.
+
+The general steps are similar to those of a regular install. Make sure your machine is :ref:`ready to build C++ and Python packages <python-install-build-deps>`, and :ref:`clone wavemap's code <python-install-clone-repo>`.
+
+However, since the build no longer happens in a virtual environment, the pip packages required to build pywavemap must also available on your machine::
+
+      pip3 install nanobind scikit-build-core
+      pip3 install typing_extensions  # Only needed if python_version < 3.11
+
+You can then install pywavemap without isolation to allow incremental rebuilds::
+
+      cd ~/wavemap/library/python
+      rm -rf build  # Only needed if you previously built pywavemap differently
+      pip install --no-build-isolation -ve .
+
+The command above needs to be run after every change to reinstall the updated package. For an even more interactive experience, use::
+
+      cd ~/wavemap/library/python
+      rm -rf build  # Only needed if you previously built pywavemap differently
+      pip install --no-build-isolation -Ceditable.rebuild=true -ve .
+
+In this mode, any code that changed is automatically rebuilt whenever pywavemap is imported into a Python session.

From 98fca95068dff7f3d7662e8a4c064c09c0799a7b Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 13:40:21 +0200
Subject: [PATCH 29/85] Improve installation instructions

---
 docs/pages/installation/cmake.rst  |  6 ++++
 docs/pages/installation/python.rst | 51 +++++++++++++++++++++---------
 2 files changed, 42 insertions(+), 15 deletions(-)

diff --git a/docs/pages/installation/cmake.rst b/docs/pages/installation/cmake.rst
index d7309fb52..e854f45ee 100644
--- a/docs/pages/installation/cmake.rst
+++ b/docs/pages/installation/cmake.rst
@@ -8,10 +8,16 @@ Wavemap's C++ library can be used as standard CMake package. In the following se
 
 Note that if you intend to use wavemap with ROS1, you can skip this guide and proceed directly to the :doc:`ROS1 installation page <ros1>`.
 
+Prerequisites
+*************
 Before you start, make sure you have the necessary tools installed to build C++ projects with CMake. On Ubuntu, we recommend installing::
 
     sudo apt install cmake build-essential git
 
+.. note::
+
+      If you are working in Docker, these dependencies are only required inside your container. Not on your host machine.
+
 FetchContent
 ************
 The fastest way to include wavemap in an existing CMake project is to use FetchContent, by adding the following lines to your project's `CMakeLists.txt`:
diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index 6811d7933..8d7ad2aa7 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -1,16 +1,16 @@
-Python
-######
+Python (pip)
+############
 .. rstcheck: ignore-directives=tab-set-code
 
-We will make pywavemap available through PyPI soon. In the meantime, you can already pip install pywavemap from sources.
+We will make pywavemap available through PyPI soon. In the meantime, you can install it directly from source using pip.
+
+If you only plan to use pywavemap without modifying its code, a regular install is easiest. However, if you are actively working on wavemap's C++ or Python libraries, we recommend using the editable installation method for fast, incremental rebuilds.
 
 Regular install
 ***************
-In case you'd like to use pywavemap but do not expect to change its code often, we recommend a regular install.
-
 .. _python-install-build-deps:
 
-First, make sure the necessary dependencies to build C++ and Python packages are available on your machine:
+First, make sure the necessary dependencies to build C++ and Python packages are available:
 
 .. tab-set-code::
 
@@ -19,16 +19,18 @@ First, make sure the necessary dependencies to build C++ and Python packages are
 
       sudo apt update
       sudo apt install git build-essential python3-dev python3-pip
+      sudo apt install python3-venv  # If you use virtual environments
 
     .. code-block:: Alpine
       :class: no-header
 
       apk update
       apk add git build-base python3-dev py3-pip
+      apk add python3-venv  # If you use virtual environments
 
 .. _python-install-clone-repo:
 
-Next, clone wavemap's code to your machine. We recommend using `SSH <https://docs.github.com/en/authentication/connecting-to-github-with-ssh>`_. Alternatively, HTTPS can be used without requiring keys to be set up.
+Next, clone wavemap's code to your machine:
 
 .. tab-set-code::
 
@@ -44,6 +46,26 @@ Next, clone wavemap's code to your machine. We recommend using `SSH <https://doc
       cd ~/
       git clone https://github.com/ethz-asl/wavemap.git
 
+.. _python-install-setup-venv:
+
+*Optional:* We recommend using a virtual environment to isolate your Python dependencies. Create and activate it with the following commands:
+
+.. tab-set-code::
+
+    .. code-block:: Debian/Ubuntu
+      :class: no-header
+
+      sudo apt install python3-venv  # If needed
+      python3 -m venv <path_to_new_virtual_env>
+      source <path_to_new_virtual_env>/bin/activate
+
+    .. code-block:: Alpine
+      :class: no-header
+
+      apk add python3-venv  # If needed
+      python3 -m venv <path_to_new_virtual_env>
+      source <path_to_new_virtual_env>/bin/activate
+
 You can then install pywavemap by running::
 
     cd ~/wavemap/library/python
@@ -51,25 +73,24 @@ You can then install pywavemap by running::
 
 Editable install
 ****************
-By default, pip will recompile wavemap's C++ and Python libraries in a virtual environment from scratch on every install. If you're interested in modifying wavemap's code, you can save time by enabling incremental builds.
+If you're interested in modifying wavemap's code, you can save time by enabling incremental builds.
 
-The general steps are similar to those of a regular install. Make sure your machine is :ref:`ready to build C++ and Python packages <python-install-build-deps>`, and :ref:`clone wavemap's code <python-install-clone-repo>`.
+The general steps are similar to those for a regular installation. Ensure your machine is :ref:`ready to build C++ and Python packages <python-install-build-deps>` and that you've :ref:`cloned the code <python-install-clone-repo>`. Optionally, you can :ref:`set up a virtual environment <python-install-setup-venv>`.
 
-However, since the build no longer happens in a virtual environment, the pip packages required to build pywavemap must also available on your machine::
+Since editable installs are no longer built in an isolated environment, all build dependencies must be available on your system::
 
       pip3 install nanobind scikit-build-core
-      pip3 install typing_extensions  # Only needed if python_version < 3.11
+      pip3 install typing_extensions  # Only needed for Python < 3.11
 
-You can then install pywavemap without isolation to allow incremental rebuilds::
+You can then install pywavemap with incremental rebuilds using::
 
       cd ~/wavemap/library/python
-      rm -rf build  # Only needed if you previously built pywavemap differently
       pip install --no-build-isolation -ve .
 
-The command above needs to be run after every change to reinstall the updated package. For an even more interactive experience, use::
+When you change wavemap's code, the command above must manually be rerun to reinstall the updated package. For a more interactive experience, you can use::
 
       cd ~/wavemap/library/python
       rm -rf build  # Only needed if you previously built pywavemap differently
       pip install --no-build-isolation -Ceditable.rebuild=true -ve .
 
-In this mode, any code that changed is automatically rebuilt whenever pywavemap is imported into a Python session.
+In this mode, code changes are automatically rebuilt whenever pywavemap is imported into a Python session.

From aa7627f5f3c3b9900ad0a71c0f37f4df1e70ceda Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 13:55:15 +0200
Subject: [PATCH 30/85] Minor adjustments to landing page text

---
 README.md            | 5 +++--
 docs/pages/intro.rst | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index a88248625..107cda7f3 100644
--- a/README.md
+++ b/README.md
@@ -20,7 +20,7 @@
 
 Wavemap achieves state-of-the-art memory and computational efficiency by combining Haar wavelet compression and a coarse-to-fine measurement integration scheme. Advanced measurement models allow it to attain exceptionally high recall rates on challenging obstacles like thin objects.
 
-The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single map.
+The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single multi-resolution occupancy grid map.
 
 ⭐ If you find wavemap useful, star it on GitHub to get notified of new releases!
 
@@ -33,7 +33,8 @@ The framework's documentation is hosted on [GitHub Pages](https://ethz-asl.githu
 * [Tutorials](https://ethz-asl.github.io/wavemap/pages/tutorials)
 * [Parameters](https://ethz-asl.github.io/wavemap/pages/parameters)
 * [Contributing](https://ethz-asl.github.io/wavemap/pages/contributing)
-* [Library API](https://ethz-asl.github.io/wavemap/cpp_api/unabridged_api)
+* [C++ API](https://ethz-asl.github.io/wavemap/cpp_api/unabridged_api)
+* [Python API](https://ethz-asl.github.io/wavemap/python_api)
 * [FAQ](https://ethz-asl.github.io/wavemap/pages/faq)
 
 ## Paper
diff --git a/docs/pages/intro.rst b/docs/pages/intro.rst
index 3e756e9af..ef9d96b65 100644
--- a/docs/pages/intro.rst
+++ b/docs/pages/intro.rst
@@ -5,7 +5,7 @@ Hierarchical, multi-resolution volumetric mapping
 *************************************************
 Wavemap achieves state-of-the-art memory and computational efficiency by combining Haar wavelet compression and a coarse-to-fine measurement integration scheme. Advanced measurement models allow it to attain exceptionally high recall rates on challenging obstacles like thin objects.
 
-The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single map.
+The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single multi-resolution occupancy grid map.
 
 Paper
 *****

From 2c7d7b45a31c668c1310b9d25938ed2fb1d8dc25 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 15:08:42 +0200
Subject: [PATCH 31/85] Minor changes to API, demo script and doc consistency

---
 docs/pages/installation/python.rst  |  4 +-
 examples/python/panoptic_mapping.py | 90 +++++++++++------------------
 library/python/src/pipeline.cc      | 12 ++--
 3 files changed, 42 insertions(+), 64 deletions(-)

diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index 8d7ad2aa7..8a977b00e 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -85,12 +85,12 @@ Since editable installs are no longer built in an isolated environment, all buil
 You can then install pywavemap with incremental rebuilds using::
 
       cd ~/wavemap/library/python
-      pip install --no-build-isolation -ve .
+      pip3 install --no-build-isolation -ve .
 
 When you change wavemap's code, the command above must manually be rerun to reinstall the updated package. For a more interactive experience, you can use::
 
       cd ~/wavemap/library/python
       rm -rf build  # Only needed if you previously built pywavemap differently
-      pip install --no-build-isolation -Ceditable.rebuild=true -ve .
+      pip3 install --no-build-isolation -Ceditable.rebuild=true -ve .
 
 In this mode, code changes are automatically rebuilt whenever pywavemap is imported into a Python session.
diff --git a/examples/python/panoptic_mapping.py b/examples/python/panoptic_mapping.py
index 9305a3731..1640e7a64 100644
--- a/examples/python/panoptic_mapping.py
+++ b/examples/python/panoptic_mapping.py
@@ -4,67 +4,34 @@
 import csv
 from PIL import Image as PilImage
 import numpy as np
+import yaml
 import pywavemap as pw
+from tqdm import tqdm
 
 
 class DataLoader():
     # pylint: disable=R0902
-    def __init__(self, data_path):
+    def __init__(self, params, data_path):
         self.data_path = data_path
 
-        self.map = pw.Map.create({
-            "type": "hashed_chunked_wavelet_octree",
-            "min_cell_width": {
-                "meters": 0.05
-            }
-        })
+        self.map = pw.Map.create(params["map"])
 
         self.pipeline = pw.Pipeline(self.map)
 
-        self.pipeline.addOperation({
-            "type": "threshold_map",
-            "once_every": {
-                "seconds": 5.0
-            }
-        })
-        self.pipeline.addOperation({
-            "type": "prune_map",
-            "once_every": {
-                "seconds": 10.0
-            }
-        })
-
-        self.pipeline.addIntegrator(
-            "dummy_integrator", {
-                "projection_model": {
-                    "type": "pinhole_camera_projector",
-                    "width": 640,
-                    "height": 480,
-                    "fx": 320.0,
-                    "fy": 320.0,
-                    "cx": 320.0,
-                    "cy": 240.0
-                },
-                "measurement_model": {
-                    "type": "continuous_ray",
-                    "range_sigma": {
-                        "meters": 0.01
-                    },
-                    "scaling_free": 0.2,
-                    "scaling_occupied": 0.4
-                },
-                "integration_method": {
-                    "type": "hashed_chunked_wavelet_integrator",
-                    "min_range": {
-                        "meters": 0.1
-                    },
-                    "max_range": {
-                        "meters": 5.0
-                    }
-                },
-            })
-
-        # setup
+        for operation in params["map_operations"]:
+            self.pipeline.addOperation(operation)
+
+        measurement_integrators = params["measurement_integrators"]
+        if len(measurement_integrators) != 1:
+            print("Expected 1 integrator to be specified. "
+                  f"Got {len(measurement_integrators)}.")
+            raise SystemExit
+        self.integrator_name, integrator_params = \
+            next(iter(measurement_integrators.items()))
+
+        self.pipeline.addIntegrator(self.integrator_name, integrator_params)
+
+        # Load list of measurements
         stamps_file = os.path.join(self.data_path, 'timestamps.csv')
         self.times = []
         self.ids = []
@@ -83,14 +50,14 @@ def __init__(self, data_path):
         self.times = sorted(self.times)
 
     def run(self):
-        while self.integrate_frame():
-            pass
+        for _ in tqdm(range(len(self.times)), desc="Integrating..."):
+            if not self.integrate_frame():
+                break
 
     def integrate_frame(self):
         # Check we're not done.
         if self.current_index >= len(self.times):
             return False
-        print(f"Integrating frame {self.current_index} of {len(self.times)}")
 
         # Get all data and publish.
         file_id = os.path.join(self.data_path, self.ids[self.current_index])
@@ -119,7 +86,7 @@ def integrate_frame(self):
                         transform[row, col] = pose_data[row * 4 + col]
         pose = pw.Pose(transform)
 
-        self.pipeline.runPipeline(["dummy_integrator"],
+        self.pipeline.runPipeline([self.integrator_name],
                                   pw.PosedImage(pose, image))
 
         self.current_index += 1
@@ -132,14 +99,25 @@ def save_map(self, path):
 
 
 if __name__ == '__main__':
+    config_dir = os.path.abspath(
+        os.path.join(__file__, "../../../interfaces/ros1/wavemap_ros/config"))
+    config_file = os.path.join(config_dir,
+                               "wavemap_panoptic_mapping_rgbd.yaml")
+    with open(config_file) as stream:
+        try:
+            config = yaml.safe_load(stream)
+        except yaml.YAMLError as exc:
+            print(exc)
+
     user_home = os.path.expanduser('~')
     panoptic_mapping_dir = os.path.join(user_home,
                                         "data/panoptic_mapping/flat_dataset")
     panoptic_mapping_seq = "run2"
     output_map_path = os.path.join(
         user_home, f"panoptic_mapping_{panoptic_mapping_seq}.wvmp")
+
     data_loader = DataLoader(
-        os.path.join(panoptic_mapping_dir, panoptic_mapping_seq))
+        config, os.path.join(panoptic_mapping_dir, panoptic_mapping_seq))
     data_loader.run()
     data_loader.save_map(output_map_path)
     del data_loader  # To avoid mem leak warnings on older Python versions
diff --git a/library/python/src/pipeline.cc b/library/python/src/pipeline.cc
index 4a80f50d6..ef8502219 100644
--- a/library/python/src/pipeline.cc
+++ b/library/python/src/pipeline.cc
@@ -25,21 +25,21 @@ void add_pipeline_bindings(nb::module_& m) {
       .def(
           "addIntegrator",
           [](Pipeline& self, const std::string& integrator_name,
-             const param::Value& params) -> void {
-            self.addIntegrator(integrator_name, params);
+             const param::Value& params) -> bool {
+            return self.addIntegrator(integrator_name, params);
           },
           nb::sig("def addIntegrator(self, integrator_name: str, "
-                  "integrator_params: dict) -> None"),
+                  "integrator_params: dict) -> bool"),
           "integrator_name"_a, "integrator_params"_a,
           "Create and register a new integrator")
       .def("clearIntegrators", &Pipeline::clearIntegrators,
            "Deregister all integrators.")
       .def(
           "addOperation",
-          [](Pipeline& self, const param::Value& params) -> void {
-            self.addOperation(params);
+          [](Pipeline& self, const param::Value& params) -> bool {
+            return self.addOperation(params);
           },
-          nb::sig("def addOperation(self, operation_params: dict) -> None"),
+          nb::sig("def addOperation(self, operation_params: dict) -> bool"),
           "operation_params"_a, "Create and register a new map operation.")
       .def("clearOperations", &Pipeline::clearOperations,
            "Deregister all map operations")

From b18b97586deec3e9d38dd78412fafcdb5bcc393f Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 15:33:15 +0200
Subject: [PATCH 32/85] Make python stub generation compatible with editable
 pip installs

---
 .gitignore                         |  1 -
 docs/pages/installation/python.rst |  2 +-
 library/python/CMakeLists.txt      | 21 +++++++++------------
 3 files changed, 10 insertions(+), 14 deletions(-)

diff --git a/.gitignore b/.gitignore
index 0a94ca157..7874f4da8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -39,7 +39,6 @@ qtcreator-*
 
 # Python
 *.pyc
-*.pyi
 
 /planning/cfg
 /planning/docs
diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index 8a977b00e..6168b6ade 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -93,4 +93,4 @@ When you change wavemap's code, the command above must manually be rerun to rein
       rm -rf build  # Only needed if you previously built pywavemap differently
       pip3 install --no-build-isolation -Ceditable.rebuild=true -ve .
 
-In this mode, code changes are automatically rebuilt whenever pywavemap is imported into a Python session.
+In this mode, code changes are automatically rebuilt whenever pywavemap is imported into a Python session. Note that the rebuild message is quite verbose. You can suppress it by passing ``-Ceditable.verbose=false`` as an additional argument to ``pip3 install``.
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 857beaee9..8c5377cd2 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -79,18 +79,15 @@ target_compile_options(_cpp_bindings PRIVATE
 install(TARGETS _cpp_bindings LIBRARY DESTINATION pywavemap)
 
 # Generate stubs
-nanobind_add_stub(pywavemap_stub
+nanobind_add_stub(pywavemap_stub INSTALL_TIME
     MODULE _cpp_bindings
-    OUTPUT "${CMAKE_SOURCE_DIR}/src/pywavemap/__init__.pyi"
-    PYTHON_PATH $<TARGET_FILE_DIR:_cpp_bindings>
-    DEPENDS _cpp_bindings)
-nanobind_add_stub(pywavemap_logging_stub
+    OUTPUT "pywavemap/__init__.pyi"
+    PYTHON_PATH "pywavemap")
+nanobind_add_stub(pywavemap_logging_stub INSTALL_TIME
     MODULE _cpp_bindings.logging
-    OUTPUT "${CMAKE_SOURCE_DIR}/src/pywavemap/logging.pyi"
-    PYTHON_PATH $<TARGET_FILE_DIR:_cpp_bindings>
-    DEPENDS _cpp_bindings)
-nanobind_add_stub(pywavemap_param_stub
+    OUTPUT "pywavemap/logging.pyi"
+    PYTHON_PATH "pywavemap")
+nanobind_add_stub(pywavemap_param_stub INSTALL_TIME
     MODULE _cpp_bindings.param
-    OUTPUT "${CMAKE_SOURCE_DIR}/src/pywavemap/param.pyi"
-    PYTHON_PATH $<TARGET_FILE_DIR:_cpp_bindings>
-    DEPENDS _cpp_bindings)
+    OUTPUT "pywavemap/param.pyi"
+    PYTHON_PATH "pywavemap")

From 10c66a9d926c05a15bcb2b9c0d004fe3c6047053 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 20 Aug 2024 18:44:08 +0200
Subject: [PATCH 33/85] Draft Python API tutorial outline

---
 docs/pages/tutorials/cpp.rst                  |   6 +-
 docs/pages/tutorials/python.rst               |  87 ++++++++++++-
 docs/pages/tutorials/ros1.rst                 |   4 +-
 examples/python/io_load_map_from_file.py      |   7 +
 examples/python/io_save_map_to_file.py        |  15 +++
 examples/python/mapping_pipeline.py           | 122 ++++++++++++++++++
 .../python/queries_accelerated_queries.py     |   2 +
 examples/python/queries_classification.py     |  54 ++++++++
 examples/python/queries_fixed_resolution.py   |  13 ++
 examples/python/queries_multi_resolution.py   |   2 +
 .../queries_nearest_neighbor_interpolation.py |  10 ++
 .../python/queries_trilinear_interpolation.py |  11 ++
 12 files changed, 330 insertions(+), 3 deletions(-)
 create mode 100644 examples/python/io_load_map_from_file.py
 create mode 100644 examples/python/io_save_map_to_file.py
 create mode 100644 examples/python/mapping_pipeline.py
 create mode 100644 examples/python/queries_accelerated_queries.py
 create mode 100644 examples/python/queries_classification.py
 create mode 100644 examples/python/queries_fixed_resolution.py
 create mode 100644 examples/python/queries_multi_resolution.py
 create mode 100644 examples/python/queries_nearest_neighbor_interpolation.py
 create mode 100644 examples/python/queries_trilinear_interpolation.py

diff --git a/docs/pages/tutorials/cpp.rst b/docs/pages/tutorials/cpp.rst
index 3d6f29a54..8980322c2 100644
--- a/docs/pages/tutorials/cpp.rst
+++ b/docs/pages/tutorials/cpp.rst
@@ -2,7 +2,11 @@ C++ API
 #######
 .. rstcheck: ignore-roles=gh_file
 
-In this tutorial, we illustrate how you can use wavemap's C++ API in your own project. Note that an example CMake package that contains all of the examples that follow can be found :gh_file:`here <examples/cpp>`.
+In this tutorial, we illustrate how you can use wavemap's C++ API in your own projects.
+
+.. note::
+
+    An example package that combines the setup steps and code examples that follow can be found :gh_file:`here <examples/cpp>`.
 
 CMake target setup
 ******************
diff --git a/docs/pages/tutorials/python.rst b/docs/pages/tutorials/python.rst
index 9d5a7aaa7..4489d6cec 100644
--- a/docs/pages/tutorials/python.rst
+++ b/docs/pages/tutorials/python.rst
@@ -1,3 +1,88 @@
 Python API
 ##########
-Wavemap's Python API is under active development. We will add it to the documentation soon.
+.. highlight:: python
+.. rstcheck: ignore-roles=gh_file
+
+In this tutorial, we illustrate how you can use wavemap's Python API in your own projects.
+
+Setup
+*****
+Before you start, make sure you :doc:`installed pywavemap <../installation/python>`. In case you used a virtual environment, activate it by running the following command from your terminal:
+
+.. code-block:: bash
+
+    source <path_to_your_virtual_env>/bin/activate
+
+In your python files, you can then load the API by simply calling::
+
+    import pywavemap
+
+Code examples
+*************
+
+In the following sections, we provide sample code for common tasks. If you'd like to request examples for additional tasks or contribute new examples, please don't hesitate to `contact us <https://github.com/ethz-asl/wavemap/issues>`_.
+
+.. note::
+
+    All of the examples scripts that follow can be found :gh_file:`here <examples/python>`.
+
+Serialization
+=============
+
+Files
+-----
+Saving maps to files:
+
+.. literalinclude:: ../../../examples/python/io_save_map_to_file.py
+    :language: python
+
+Loading maps from files:
+
+.. literalinclude:: ../../../examples/python/io_load_map_from_file.py
+    :language: python
+
+Queries
+=======
+
+Fixed resolution
+----------------
+.. literalinclude:: ../../../examples/python/queries_fixed_resolution.py
+    :language: python
+
+Multi-res averages
+------------------
+.. literalinclude:: ../../../examples/python/queries_multi_resolution.py
+    :language: python
+
+Accelerators
+------------
+.. literalinclude:: ../../../examples/python/queries_accelerated_queries.py
+    :language: python
+
+Interpolation
+-------------
+
+Nearest neighbor interpolation:
+
+.. literalinclude:: ../../../examples/python/queries_nearest_neighbor_interpolation.py
+    :language: python
+
+Trilinear interpolation:
+
+.. literalinclude:: ../../../examples/python/queries_trilinear_interpolation.py
+    :language: python
+
+Classification
+--------------
+
+.. literalinclude:: ../../../examples/python/queries_classification.py
+    :language: python
+
+Mapping
+=======
+
+Full pipeline
+-------------
+
+.. literalinclude:: ../../../examples/python/mapping_pipeline.py
+    :language: python
diff --git a/docs/pages/tutorials/ros1.rst b/docs/pages/tutorials/ros1.rst
index d6ae496fc..588f67afc 100644
--- a/docs/pages/tutorials/ros1.rst
+++ b/docs/pages/tutorials/ros1.rst
@@ -30,7 +30,9 @@ Your own code
 *************
 We now briefly discuss how to set up your own ROS1 package to use wavemap, before proceeding to code examples.
 
-Note that a working example package that combines this tutorial's setup steps and code examples can be found :gh_file:`here <examples/ros1>`.
+.. note::
+
+    An example package that combines the setup steps and code examples that follow can be found :gh_file:`here <examples/ros1>`.
 
 Build configuration
 ===================
diff --git a/examples/python/io_load_map_from_file.py b/examples/python/io_load_map_from_file.py
new file mode 100644
index 000000000..20d414ec4
--- /dev/null
+++ b/examples/python/io_load_map_from_file.py
@@ -0,0 +1,7 @@
+import os
+import pywavemap as pw
+
+# Load the map
+user_home = os.path.expanduser('~')
+map_path = os.path.join(user_home, "your_map.wvmp")
+your_map = pw.Map.load(map_path)
diff --git a/examples/python/io_save_map_to_file.py b/examples/python/io_save_map_to_file.py
new file mode 100644
index 000000000..309d757c8
--- /dev/null
+++ b/examples/python/io_save_map_to_file.py
@@ -0,0 +1,15 @@
+import os
+import pywavemap as pw
+
+# Create an empty map for illustration purposes
+your_map = pw.Map.create({
+    "type": "hashed_chunked_wavelet_octree",
+    "min_cell_width": {
+        "meters": 0.1
+    }
+})
+
+# Save the map
+user_home = os.path.expanduser('~')
+map_path = os.path.join(user_home, "your_map.wvmp")
+your_map.store(map_path)
diff --git a/examples/python/mapping_pipeline.py b/examples/python/mapping_pipeline.py
new file mode 100644
index 000000000..e57e71f90
--- /dev/null
+++ b/examples/python/mapping_pipeline.py
@@ -0,0 +1,122 @@
+# !/usr/bin/env python3
+
+import os
+import csv
+from PIL import Image as PilImage
+import numpy as np
+import pywavemap as pw
+
+# Parameters
+home_dir = os.path.expanduser('~')
+measurement_dir = os.path.join(home_dir,
+                               "data/panoptic_mapping/flat_dataset/run2")
+output_map_path = os.path.join(home_dir, "your_map.wvmp")
+
+# Create a map
+your_map = pw.Map.create({
+    "type": "hashed_chunked_wavelet_octree",
+    "min_cell_width": {
+        "meters": 0.05
+    }
+})
+
+# Create a measurement integration pipeline
+pipeline = pw.Pipeline(your_map)
+# Add map operations
+pipeline.addOperation({
+    "type": "threshold_map",
+    "once_every": {
+        "seconds": 5.0
+    }
+})
+pipeline.addOperation({"type": "prune_map", "once_every": {"seconds": 10.0}})
+# Add a measurement integrator
+pipeline.addIntegrator(
+    "my_integrator", {
+        "projection_model": {
+            "type": "pinhole_camera_projector",
+            "width": 640,
+            "height": 480,
+            "fx": 320.0,
+            "fy": 320.0,
+            "cx": 320.0,
+            "cy": 240.0
+        },
+        "measurement_model": {
+            "type": "continuous_ray",
+            "range_sigma": {
+                "meters": 0.01
+            },
+            "scaling_free": 0.2,
+            "scaling_occupied": 0.4
+        },
+        "integration_method": {
+            "type": "hashed_chunked_wavelet_integrator",
+            "min_range": {
+                "meters": 0.1
+            },
+            "max_range": {
+                "meters": 5.0
+            }
+        },
+    })
+
+# Index the input data
+ids = []
+times = []
+stamps_file = os.path.join(measurement_dir, 'timestamps.csv')
+if not os.path.isfile(stamps_file):
+    print(f"Could not find timestamp file '{stamps_file}'.")
+with open(stamps_file, 'r') as read_obj:
+    csv_reader = csv.reader(read_obj)
+    for row in csv_reader:
+        if row[0] == "ImageID":
+            continue
+        ids.append(str(row[0]))
+        times.append(float(row[1]) / 1e9)
+ids = [x for _, x in sorted(zip(times, ids))]
+
+# Integrate all the measurements
+current_index = 0
+while True:
+    # Check we're not done
+    if current_index >= len(ids):
+        break
+
+    # Load depth image
+    file_path_prefix = os.path.join(measurement_dir, ids[current_index])
+    depth_file = file_path_prefix + "_depth.tiff"
+    if not os.path.isfile(depth_file):
+        print(f"Could not find depth image file '{depth_file}'")
+        current_index += 1
+        raise SystemExit
+    cv_img = PilImage.open(depth_file)
+    image = pw.Image(np.array(cv_img).transpose())
+
+    # Load transform
+    pose_file = file_path_prefix + "_pose.txt"
+    if not os.path.isfile(pose_file):
+        print(f"Could not find pose file '{pose_file}'")
+        current_index += 1
+        raise SystemExit
+    if os.path.isfile(pose_file):
+        with open(pose_file, 'r') as f:
+            pose_data = [float(x) for x in f.read().split()]
+            transform = np.eye(4)
+            for row in range(4):
+                for col in range(4):
+                    transform[row, col] = pose_data[row * 4 + col]
+    pose = pw.Pose(transform)
+
+    # Integrate the depth image
+    print(f"Integrating measurement {ids[current_index]}")
+    pipeline.runPipeline(["my_integrator"], pw.PosedImage(pose, image))
+
+    current_index += 1
+
+# Save the map
+print(f"Saving map of size {your_map.memory_usage}")
+your_map.store(output_map_path)
+
+# Avoids leak warnings on old Python versions with lazy garbage collectors
+del pipeline, your_map
diff --git a/examples/python/queries_accelerated_queries.py b/examples/python/queries_accelerated_queries.py
new file mode 100644
index 000000000..a515c072c
--- /dev/null
+++ b/examples/python/queries_accelerated_queries.py
@@ -0,0 +1,2 @@
+# TODO(victorr): Extend the bindings
+# TODO(victorr): Include vectorized version
diff --git a/examples/python/queries_classification.py b/examples/python/queries_classification.py
new file mode 100644
index 000000000..470f251e8
--- /dev/null
+++ b/examples/python/queries_classification.py
@@ -0,0 +1,54 @@
+import numpy as np
+
+
+def logOddsToProbability(log_odds):
+    odds = np.exp(log_odds)
+    prob = odds / (1.0 + odds)
+    return prob
+
+
+def probabilityToLogOdds(probability):
+    odds = probability / (1.0 - probability)
+    return np.log(odds)
+
+
+# Declare a floating point value representing the occupancy posterior in log
+# odds as queried from the map in one of the previous examples
+# pylint: disable=wrong-import-position
+from queries_trilinear_interpolation import occupancy_log_odds
+
+# A point is considered unobserved if its occupancy posterior is equal to the
+# prior. Wavemap assumes that an unobserved point is equally likely to be
+# free or occupied. In other words, the prior occupancy probability is 0.5,
+# which corresponds to a log odds value of 0.0. Accounting for numerical
+# noise, checking whether a point is unobserved can be done as follows:
+kUnobservedThreshold = 1e-3
+is_unobserved = np.abs(occupancy_log_odds) < kUnobservedThreshold
+print(is_unobserved)
+
+# In case you would like to convert log odds into probabilities, we provide
+# the following convenience function:
+occupancy_probability = logOddsToProbability(occupancy_log_odds)
+print(occupancy_probability)
+
+# To classify whether a point is estimated to be occupied or free, you need
+# to choose a discrimination threshold. A reasonable default threshold is 0.5
+# (probability), which corresponds to 0.0 log odds.
+kOccupancyThresholdProb = 0.5
+kOccupancyThresholdLogOdds = 0.0
+
+# NOTE: To tailor the threshold, we recommend running wavemap on a dataset
+#       that is representative of your application and analyzing the Receiver
+#       Operating Characteristic curve.
+
+# Once a threshold has been chosen, you can either classify in log space
+is_occupied = kOccupancyThresholdLogOdds < occupancy_log_odds
+is_free = occupancy_log_odds < kOccupancyThresholdLogOdds
+print(is_occupied)
+print(is_free)
+
+# Or in probability space
+is_occupied = kOccupancyThresholdProb < occupancy_probability
+is_free = occupancy_probability < kOccupancyThresholdProb
+print(is_occupied)
+print(is_free)
diff --git a/examples/python/queries_fixed_resolution.py b/examples/python/queries_fixed_resolution.py
new file mode 100644
index 000000000..88add7274
--- /dev/null
+++ b/examples/python/queries_fixed_resolution.py
@@ -0,0 +1,13 @@
+import numpy as np
+
+# Load a map
+from io_load_map_from_file import your_map
+
+# Declare the index to query
+query_index = np.array([0, 0, 0])
+
+# Query the map's value at the given index
+occupancy_log_odds = your_map.getCellValue(query_index)
+print(occupancy_log_odds)
+
+# TODO(victorr): Extend bindings with vectorized version
diff --git a/examples/python/queries_multi_resolution.py b/examples/python/queries_multi_resolution.py
new file mode 100644
index 000000000..a515c072c
--- /dev/null
+++ b/examples/python/queries_multi_resolution.py
@@ -0,0 +1,2 @@
+# TODO(victorr): Extend the bindings
+# TODO(victorr): Include vectorized version
diff --git a/examples/python/queries_nearest_neighbor_interpolation.py b/examples/python/queries_nearest_neighbor_interpolation.py
new file mode 100644
index 000000000..483367aad
--- /dev/null
+++ b/examples/python/queries_nearest_neighbor_interpolation.py
@@ -0,0 +1,10 @@
+import numpy as np
+
+# Load a map
+# from io_load_map_from_file import your_map
+
+# Declare the point to query [in map frame]
+query_point = np.array([0.4, .5, 0.6])
+
+# TODO(victorr): Extend the bindings
+# TODO(victorr): Include vectorized version
diff --git a/examples/python/queries_trilinear_interpolation.py b/examples/python/queries_trilinear_interpolation.py
new file mode 100644
index 000000000..866c8e06b
--- /dev/null
+++ b/examples/python/queries_trilinear_interpolation.py
@@ -0,0 +1,11 @@
+import numpy as np
+
+# Load a map
+# from io_load_map_from_file import your_map
+
+# Declare the point to query [in map frame]
+query_point = np.array([0.4, .5, 0.6])
+
+# TODO(victorr): Extend the bindings
+# TODO(victorr): Include vectorized version
+occupancy_log_odds = 0.0  # placeholder

From 108c03208cae77cbf4035d569c478043a742aa1f Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 21 Aug 2024 17:43:57 +0200
Subject: [PATCH 34/85] Explain code samples and add CSS for deeper subsections
 in docs

---
 docs/_static/custom.css                       |  5 ++
 docs/conf.py                                  |  2 +-
 docs/pages/tutorials/cpp.rst                  | 39 ++++++++++---
 docs/pages/tutorials/python.rst               | 57 +++++++++++++------
 .../load_map_from_file.py}                    |  0
 .../save_map_to_file.py}                      |  0
 .../full_pipeline.py}                         |  2 -
 examples/python/queries/_dummy_objects.py     | 14 +++++
 .../accelerated_queries.py}                   |  0
 .../classification.py}                        | 33 ++++++-----
 .../fixed_resolution.py}                      |  5 +-
 .../multi_resolution.py}                      |  0
 .../nearest_neighbor_interpolation.py}        |  0
 .../trilinear_interpolation.py}               |  0
 14 files changed, 110 insertions(+), 47 deletions(-)
 create mode 100644 docs/_static/custom.css
 rename examples/python/{io_load_map_from_file.py => io/load_map_from_file.py} (100%)
 rename examples/python/{io_save_map_to_file.py => io/save_map_to_file.py} (100%)
 rename examples/python/{mapping_pipeline.py => mapping/full_pipeline.py} (99%)
 create mode 100644 examples/python/queries/_dummy_objects.py
 rename examples/python/{queries_accelerated_queries.py => queries/accelerated_queries.py} (100%)
 rename examples/python/{queries_classification.py => queries/classification.py} (90%)
 rename examples/python/{queries_fixed_resolution.py => queries/fixed_resolution.py} (68%)
 rename examples/python/{queries_multi_resolution.py => queries/multi_resolution.py} (100%)
 rename examples/python/{queries_nearest_neighbor_interpolation.py => queries/nearest_neighbor_interpolation.py} (100%)
 rename examples/python/{queries_trilinear_interpolation.py => queries/trilinear_interpolation.py} (100%)

diff --git a/docs/_static/custom.css b/docs/_static/custom.css
new file mode 100644
index 000000000..b559aaaf4
--- /dev/null
+++ b/docs/_static/custom.css
@@ -0,0 +1,5 @@
+#content h5 {
+    font-weight: 600;
+    line-height: 1.75rem;
+    margin-top: 1.5rem
+}
diff --git a/docs/conf.py b/docs/conf.py
index fcfcd30f4..00e428771 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -53,7 +53,7 @@
     "mode": "production",
 }
 html_static_path = ["_static"]
-html_css_files = []
+html_css_files = ["custom.css"]
 html_js_files = []
 
 # Theme specific options
diff --git a/docs/pages/tutorials/cpp.rst b/docs/pages/tutorials/cpp.rst
index 8980322c2..91fb68a9f 100644
--- a/docs/pages/tutorials/cpp.rst
+++ b/docs/pages/tutorials/cpp.rst
@@ -58,41 +58,62 @@ Loading maps from files:
 
 Queries
 =======
+In this section, we illustrate how you can query the map and classify whether a point or region of interest is occupied.
+
+Node indices
+------------
+The map models the environment by filling it with cubes of variable sizes, arranged as the nodes of an octree. Node indices are defined as integer [X, Y, Z, height] coordinates, whose XYZ values correspond to the node's position in the octree's grid at the given *height*, or level in the tree. Height 0 corresponds to the map's maximum resolution, and the grid resolution is halved for each subsequent height level.
 
 Fixed resolution
-----------------
+^^^^^^^^^^^^^^^^
+Querying the value of a single node in the highest resolution grid (*height=0*) can be done as follows.
+
 .. literalinclude:: ../../../examples/cpp/queries/fixed_resolution.cc
     :language: c++
 
 Multi-res averages
-------------------
+^^^^^^^^^^^^^^^^^^
+It is also possible to query lower resolution nodes, whose values correspond to the average estimated occupancy of the volume they cover.
+
 .. literalinclude:: ../../../examples/cpp/queries/multi_resolution.cc
     :language: c++
 
 Accelerators
-------------
+^^^^^^^^^^^^
+In case you intend to look up multiple node values, we recommend using wavemap's query accelerator which traverses the octree significantly faster by caching parent nodes.
+
 .. literalinclude:: ../../../examples/cpp/queries/accelerated_queries.cc
     :language: c++
 
 .. _cpp-code-examples-interpolation:
 
-Interpolation
--------------
+Real coordinates
+----------------
+Many applications require occupancy estimates at arbitrary 3D points, with real-valued coordinates. Such estimates are computed by interpolating the map.
+
+.. note::
+
+    In case the query points are expressed in a different coordinate frame than the map, do not forget to transform them into the map frame before you continue.
 
-Nearest neighbor interpolation:
+Nearest neighbor interpolation
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+The simplest form of interpolation simply looks up the value of the map node that is closest to the query point.
 
 .. literalinclude:: ../../../examples/cpp/queries/nearest_neighbor_interpolation.cc
     :language: c++
 
-Trilinear interpolation:
+Trilinear interpolation
+^^^^^^^^^^^^^^^^^^^^^^^
+Another option is to linearly interpolate the map along the x, y, and z axes. This method produces cleaner, more accurate results at the cost of being slightly slower, since it needs to query 8 neighboring map nodes.
 
 .. literalinclude:: ../../../examples/cpp/queries/trilinear_interpolation.cc
     :language: c++
 
 .. _cpp-code-examples-classification:
 
-Classification
---------------
+Occupancy classification
+------------------------
+Once the estimated occupancy at a node or point has been retrieved, it can be classified as follows.
 
 .. literalinclude:: ../../../examples/cpp/queries/classification.cc
     :language: c++
diff --git a/docs/pages/tutorials/python.rst b/docs/pages/tutorials/python.rst
index 4489d6cec..d0c399ebe 100644
--- a/docs/pages/tutorials/python.rst
+++ b/docs/pages/tutorials/python.rst
@@ -33,49 +33,70 @@ Files
 -----
 Saving maps to files:
 
-.. literalinclude:: ../../../examples/python/io_save_map_to_file.py
+.. literalinclude:: ../../../examples/python/io/save_map_to_file.py
     :language: python
 
 Loading maps from files:
 
-.. literalinclude:: ../../../examples/python/io_load_map_from_file.py
+.. literalinclude:: ../../../examples/python/io/load_map_from_file.py
     :language: python
 
 Queries
 =======
+In this section, we illustrate how you can query the map and classify whether a point or region of interest is occupied.
+
+Node indices
+------------
+The map models the environment by filling it with cubes of variable sizes, arranged as the nodes of an octree. Node indices are defined as integer [X, Y, Z, height] coordinates, whose XYZ values correspond to the node's position in the octree's grid at the given *height*, or level in the tree. Height 0 corresponds to the map's maximum resolution, and the grid resolution is halved for each subsequent height level.
 
 Fixed resolution
-----------------
-.. literalinclude:: ../../../examples/python/queries_fixed_resolution.py
+^^^^^^^^^^^^^^^^
+Querying the value of a single node in the highest resolution grid (*height=0*) can be done as follows.
+
+.. literalinclude:: ../../../examples/python/queries/fixed_resolution.py
     :language: python
 
 Multi-res averages
-------------------
-.. literalinclude:: ../../../examples/python/queries_multi_resolution.py
+^^^^^^^^^^^^^^^^^^
+It is also possible to query lower resolution nodes, whose values correspond to the average estimated occupancy of the volume they cover.
+
+.. literalinclude:: ../../../examples/python/queries/multi_resolution.py
     :language: python
 
 Accelerators
-------------
-.. literalinclude:: ../../../examples/python/queries_accelerated_queries.py
+^^^^^^^^^^^^
+In case you intend to look up multiple node values, we recommend using wavemap's query accelerator which traverses the octree significantly faster by caching parent nodes.
+
+.. literalinclude:: ../../../examples/python/queries/accelerated_queries.py
     :language: python
 
-Interpolation
--------------
+Real coordinates
+----------------
+Many applications require occupancy estimates at arbitrary 3D points, with real-valued coordinates. Such estimates are computed by interpolating the map.
+
+.. note::
+
+    In case the query points are expressed in a different coordinate frame than the map, do not forget to transform them into the map frame before you continue.
 
-Nearest neighbor interpolation:
+Nearest neighbor interpolation
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+The simplest form of interpolation simply looks up the value of the map node that is closest to the query point.
 
-.. literalinclude:: ../../../examples/python/queries_nearest_neighbor_interpolation.py
+.. literalinclude:: ../../../examples/python/queries/nearest_neighbor_interpolation.py
     :language: python
 
-Trilinear interpolation:
+Trilinear interpolation
+^^^^^^^^^^^^^^^^^^^^^^^
+Another option is to linearly interpolate the map along the x, y, and z axes. This method produces cleaner, more accurate results at the cost of being slightly slower, since it needs to query 8 neighboring map nodes.
 
-.. literalinclude:: ../../../examples/python/queries_trilinear_interpolation.py
+.. literalinclude:: ../../../examples/python/queries/trilinear_interpolation.py
     :language: python
 
-Classification
---------------
+Occupancy classification
+------------------------
+Once the estimated occupancy at a node or point has been retrieved, it can be classified as follows.
 
-.. literalinclude:: ../../../examples/python/queries_classification.py
+.. literalinclude:: ../../../examples/python/queries/classification.py
     :language: python
 
 Mapping
@@ -84,5 +105,5 @@ Mapping
 Full pipeline
 -------------
 
-.. literalinclude:: ../../../examples/python/mapping_pipeline.py
+.. literalinclude:: ../../../examples/python/mapping/full_pipeline.py
     :language: python
diff --git a/examples/python/io_load_map_from_file.py b/examples/python/io/load_map_from_file.py
similarity index 100%
rename from examples/python/io_load_map_from_file.py
rename to examples/python/io/load_map_from_file.py
diff --git a/examples/python/io_save_map_to_file.py b/examples/python/io/save_map_to_file.py
similarity index 100%
rename from examples/python/io_save_map_to_file.py
rename to examples/python/io/save_map_to_file.py
diff --git a/examples/python/mapping_pipeline.py b/examples/python/mapping/full_pipeline.py
similarity index 99%
rename from examples/python/mapping_pipeline.py
rename to examples/python/mapping/full_pipeline.py
index e57e71f90..46600f349 100644
--- a/examples/python/mapping_pipeline.py
+++ b/examples/python/mapping/full_pipeline.py
@@ -1,5 +1,3 @@
-# !/usr/bin/env python3
-
 import os
 import csv
 from PIL import Image as PilImage
diff --git a/examples/python/queries/_dummy_objects.py b/examples/python/queries/_dummy_objects.py
new file mode 100644
index 000000000..9f88e05e9
--- /dev/null
+++ b/examples/python/queries/_dummy_objects.py
@@ -0,0 +1,14 @@
+import pywavemap as pw
+
+
+def example_occupancy_log_odds():
+    return 0.0
+
+
+def example_map():
+    return pw.Map.create({
+        "type": "hashed_chunked_wavelet_octree",
+        "min_cell_width": {
+            "meters": 0.1
+        }
+    })
diff --git a/examples/python/queries_accelerated_queries.py b/examples/python/queries/accelerated_queries.py
similarity index 100%
rename from examples/python/queries_accelerated_queries.py
rename to examples/python/queries/accelerated_queries.py
diff --git a/examples/python/queries_classification.py b/examples/python/queries/classification.py
similarity index 90%
rename from examples/python/queries_classification.py
rename to examples/python/queries/classification.py
index 470f251e8..1c2c017ee 100644
--- a/examples/python/queries_classification.py
+++ b/examples/python/queries/classification.py
@@ -1,21 +1,9 @@
 import numpy as np
-
-
-def logOddsToProbability(log_odds):
-    odds = np.exp(log_odds)
-    prob = odds / (1.0 + odds)
-    return prob
-
-
-def probabilityToLogOdds(probability):
-    odds = probability / (1.0 - probability)
-    return np.log(odds)
-
+import _dummy_objects
 
 # Declare a floating point value representing the occupancy posterior in log
 # odds as queried from the map in one of the previous examples
-# pylint: disable=wrong-import-position
-from queries_trilinear_interpolation import occupancy_log_odds
+occupancy_log_odds = _dummy_objects.example_occupancy_log_odds()
 
 # A point is considered unobserved if its occupancy posterior is equal to the
 # prior. Wavemap assumes that an unobserved point is equally likely to be
@@ -26,11 +14,28 @@ def probabilityToLogOdds(probability):
 is_unobserved = np.abs(occupancy_log_odds) < kUnobservedThreshold
 print(is_unobserved)
 
+
 # In case you would like to convert log odds into probabilities, we provide
 # the following convenience function:
+def logOddsToProbability(log_odds):
+    odds = np.exp(log_odds)
+    prob = odds / (1.0 + odds)
+    return prob
+
+
 occupancy_probability = logOddsToProbability(occupancy_log_odds)
 print(occupancy_probability)
 
+
+# To do the opposite
+def probabilityToLogOdds(probability):
+    odds = probability / (1.0 - probability)
+    return np.log(odds)
+
+
+occupancy_log_odds = probabilityToLogOdds(occupancy_probability)
+print(occupancy_log_odds)
+
 # To classify whether a point is estimated to be occupied or free, you need
 # to choose a discrimination threshold. A reasonable default threshold is 0.5
 # (probability), which corresponds to 0.0 log odds.
diff --git a/examples/python/queries_fixed_resolution.py b/examples/python/queries/fixed_resolution.py
similarity index 68%
rename from examples/python/queries_fixed_resolution.py
rename to examples/python/queries/fixed_resolution.py
index 88add7274..e3c8159d9 100644
--- a/examples/python/queries_fixed_resolution.py
+++ b/examples/python/queries/fixed_resolution.py
@@ -1,7 +1,8 @@
 import numpy as np
+import _dummy_objects
 
 # Load a map
-from io_load_map_from_file import your_map
+your_map = _dummy_objects.example_map()
 
 # Declare the index to query
 query_index = np.array([0, 0, 0])
@@ -9,5 +10,3 @@
 # Query the map's value at the given index
 occupancy_log_odds = your_map.getCellValue(query_index)
 print(occupancy_log_odds)
-
-# TODO(victorr): Extend bindings with vectorized version
diff --git a/examples/python/queries_multi_resolution.py b/examples/python/queries/multi_resolution.py
similarity index 100%
rename from examples/python/queries_multi_resolution.py
rename to examples/python/queries/multi_resolution.py
diff --git a/examples/python/queries_nearest_neighbor_interpolation.py b/examples/python/queries/nearest_neighbor_interpolation.py
similarity index 100%
rename from examples/python/queries_nearest_neighbor_interpolation.py
rename to examples/python/queries/nearest_neighbor_interpolation.py
diff --git a/examples/python/queries_trilinear_interpolation.py b/examples/python/queries/trilinear_interpolation.py
similarity index 100%
rename from examples/python/queries_trilinear_interpolation.py
rename to examples/python/queries/trilinear_interpolation.py

From 865389cbbd358acf436040a8828b36c2f1016adb Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 21 Aug 2024 17:45:31 +0200
Subject: [PATCH 35/85] Forbid shallow copying of wavemap maps

---
 .../include/wavemap/core/map/hashed_chunked_wavelet_octree.h  | 3 +++
 library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h  | 3 +++
 .../cpp/include/wavemap/core/utils/query/query_accelerator.h  | 4 +++-
 library/cpp/include/wavemap/pipeline/pipeline.h               | 3 +--
 4 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h
index 4b3ce8298..c16bc40a8 100644
--- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h
+++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree.h
@@ -76,6 +76,9 @@ class HashedChunkedWaveletOctree : public MapBase {
       const HashedChunkedWaveletOctreeConfig& config)
       : MapBase(config), config_(config.checkValid()) {}
 
+  // Copy construction is not supported
+  HashedChunkedWaveletOctree(const HashedChunkedWaveletOctree&) = delete;
+
   bool empty() const override { return block_map_.empty(); }
   size_t size() const override;
   void threshold() override;
diff --git a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h
index 764012102..986792a8f 100644
--- a/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h
+++ b/library/cpp/include/wavemap/core/map/hashed_wavelet_octree.h
@@ -71,6 +71,9 @@ class HashedWaveletOctree : public MapBase {
   explicit HashedWaveletOctree(const HashedWaveletOctreeConfig& config)
       : MapBase(config), config_(config.checkValid()) {}
 
+  // Copy construction is not supported
+  HashedWaveletOctree(const HashedWaveletOctree&) = delete;
+
   bool empty() const override { return block_map_.empty(); }
   size_t size() const override;
   void threshold() override;
diff --git a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
index 8c4a17df6..50cf09481 100644
--- a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
+++ b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
@@ -16,7 +16,9 @@ class QueryAccelerator {};
 
 // Template deduction guide
 template <typename T>
-QueryAccelerator(T type) -> QueryAccelerator<T>;
+QueryAccelerator(T& type) -> QueryAccelerator<T>;
+template <typename T>
+QueryAccelerator(const T& type) -> QueryAccelerator<T>;
 
 // Query accelerator for vanilla spatial hashes
 template <typename BlockDataT, int dim>
diff --git a/library/cpp/include/wavemap/pipeline/pipeline.h b/library/cpp/include/wavemap/pipeline/pipeline.h
index 03d21201f..2bc5071e5 100644
--- a/library/cpp/include/wavemap/pipeline/pipeline.h
+++ b/library/cpp/include/wavemap/pipeline/pipeline.h
@@ -29,9 +29,8 @@ class Pipeline {
         thread_pool_(thread_pool ? std::move(thread_pool)
                                  : std::make_shared<ThreadPool>()) {}
 
-  // Indicate that copy or move-construction are not supported
+  // Copy construction is not supported
   Pipeline(const Pipeline&) = delete;
-  Pipeline(Pipeline&&) = delete;
 
   //! Deregister all measurement integrators and map operations
   void clear();

From df12fc8e7839cd0cef75e42105596b8bdc9e913b Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 21 Aug 2024 19:14:53 +0200
Subject: [PATCH 36/85] Add Python code example on querying multi-res averages

---
 docs/pages/installation/python.rst            |   1 +
 docs/python_api/index.rst                     |  30 ++++-
 examples/python/queries/multi_resolution.py   |  23 +++-
 .../wavemap/core/indexing/ndtree_index.h      |   8 ++
 library/python/CMakeLists.txt                 |   3 +-
 library/python/include/pywavemap/indices.h    |  12 ++
 .../include/pywavemap/{map.h => maps.h}       |   6 +-
 library/python/src/indices.cc                 |  36 ++++++
 library/python/src/map.cc                     |  81 -------------
 library/python/src/maps.cc                    | 111 ++++++++++++++++++
 library/python/src/measurements.cc            |  19 +--
 library/python/src/param.cc                   |   2 +-
 library/python/src/pipeline.cc                |   2 +-
 library/python/src/pywavemap.cc               |   8 +-
 library/python/src/pywavemap/__init__.py      |  19 ++-
 library/python/src/pywavemap/convert.py       |  45 +++++++
 16 files changed, 303 insertions(+), 103 deletions(-)
 create mode 100644 library/python/include/pywavemap/indices.h
 rename library/python/include/pywavemap/{map.h => maps.h} (63%)
 create mode 100644 library/python/src/indices.cc
 delete mode 100644 library/python/src/map.cc
 create mode 100644 library/python/src/maps.cc
 create mode 100644 library/python/src/pywavemap/convert.py

diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index 6168b6ade..a7a58ab67 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -1,5 +1,6 @@
 Python (pip)
 ############
+.. highlight:: bash
 .. rstcheck: ignore-directives=tab-set-code
 
 We will make pywavemap available through PyPI soon. In the meantime, you can install it directly from source using pip.
diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst
index 6530d7727..892765a97 100644
--- a/docs/python_api/index.rst
+++ b/docs/python_api/index.rst
@@ -8,15 +8,41 @@ Python API
 
 .. autoclass:: pywavemap.Map
     :members:
+.. autoclass:: pywavemap.HashedWaveletOctree
+    :show-inheritance:
+    :members:
+.. autoclass:: pywavemap.HashedChunkedWaveletOctree
+    :show-inheritance:
+    :members:
+
+.. autoclass:: pywavemap.OctreeIndex
+    :members:
+
+.. autoclass:: pywavemap.Rotation
+    :members:
+.. autoclass:: pywavemap.Pose
+    :members:
+
+.. autoclass:: pywavemap.Pointcloud
+    :members:
+.. autoclass:: pywavemap.PosedPointcloud
+    :members:
+
+.. autoclass:: pywavemap.Image
+    :members:
+.. autoclass:: pywavemap.PosedImage
+    :members:
+
 .. autoclass:: pywavemap.Pipeline
     :members:
 
-.. automodule:: pywavemap.logging
+.. automodule:: pywavemap.convert
+    :members:
 
+.. automodule:: pywavemap.logging
 .. automethod:: pywavemap.logging.set_level
 .. automethod:: pywavemap.logging.enable_prefix
 
 .. automodule:: pywavemap.param
-
 .. autoclass:: pywavemap.param.Value
     :members:
diff --git a/examples/python/queries/multi_resolution.py b/examples/python/queries/multi_resolution.py
index a515c072c..0713d6804 100644
--- a/examples/python/queries/multi_resolution.py
+++ b/examples/python/queries/multi_resolution.py
@@ -1,2 +1,21 @@
-# TODO(victorr): Extend the bindings
-# TODO(victorr): Include vectorized version
+import numpy as np
+import pywavemap as pw
+import _dummy_objects
+
+# Load a map
+your_map = _dummy_objects.example_map()
+
+# Define the center point and the minimum width of the octree cell to query
+query_point = np.array([0.4, 0.5, 0.6])
+query_min_cell_width = 0.5  # in meters
+
+# Convert it to an octree node index
+map_min_cell_width = your_map.min_cell_width
+query_height = pw.convert.cell_width_to_height(query_min_cell_width,
+                                               map_min_cell_width)
+query_index = pw.convert.point_to_node_index(query_point, map_min_cell_width,
+                                             query_height)
+
+# Query the map
+occupancy_log_odds = your_map.getCellValue(query_index)
+print(occupancy_log_odds)
diff --git a/library/cpp/include/wavemap/core/indexing/ndtree_index.h b/library/cpp/include/wavemap/core/indexing/ndtree_index.h
index ccbaa0b37..5e10da9d0 100644
--- a/library/cpp/include/wavemap/core/indexing/ndtree_index.h
+++ b/library/cpp/include/wavemap/core/indexing/ndtree_index.h
@@ -21,7 +21,12 @@ struct NdtreeIndex {
   static constexpr RelativeChild kNumChildren = int_math::exp2(dim);
   using ChildArray = std::array<NdtreeIndex, kNumChildren>;
 
+  //! The node's resolution level in the octree
+  //! @note A height of 0 corresponds to the map’s maximum resolution.
+  //!       The node's size is doubled each time the height is increased by 1.
   Element height = 0;
+  //! The node's XYZ position in the octree’s grid at the resolution level set
+  //! by *height*
   Position position = Position::Zero();
 
   bool operator==(const NdtreeIndex& other) const {
@@ -31,9 +36,12 @@ struct NdtreeIndex {
     return !(*this == other);  // NOLINT
   }
 
+  //! Compute the index of the node's direct parent
   NdtreeIndex computeParentIndex() const;
+  //! Compute the index of the node's parent (or ancestor) at *parent_height*
   NdtreeIndex computeParentIndex(Element parent_height) const;
 
+  //! Compute the index of the node's n-th child
   NdtreeIndex computeChildIndex(RelativeChild relative_child_index) const;
   ChildArray computeChildIndices() const;
 
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 8c5377cd2..000ccf3f0 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -62,8 +62,9 @@ find_package(nanobind CONFIG REQUIRED)
 # Compile our extension
 nanobind_add_module(_cpp_bindings STABLE_ABI
     src/pywavemap.cc
+    src/indices.cc
     src/logging.cc
-    src/map.cc
+    src/maps.cc
     src/measurements.cc
     src/param.cc
     src/pipeline.cc)
diff --git a/library/python/include/pywavemap/indices.h b/library/python/include/pywavemap/indices.h
new file mode 100644
index 000000000..c7a1ccd0a
--- /dev/null
+++ b/library/python/include/pywavemap/indices.h
@@ -0,0 +1,12 @@
+#ifndef PYWAVEMAP_INDICES_H_
+#define PYWAVEMAP_INDICES_H_
+
+#include <nanobind/nanobind.h>
+
+namespace nb = nanobind;
+
+namespace wavemap {
+void add_index_bindings(nb::module_& m);
+}  // namespace wavemap
+
+#endif  // PYWAVEMAP_INDICES_H_
diff --git a/library/python/include/pywavemap/map.h b/library/python/include/pywavemap/maps.h
similarity index 63%
rename from library/python/include/pywavemap/map.h
rename to library/python/include/pywavemap/maps.h
index e6b99d7ab..be1320cd8 100644
--- a/library/python/include/pywavemap/map.h
+++ b/library/python/include/pywavemap/maps.h
@@ -1,5 +1,5 @@
-#ifndef PYWAVEMAP_MAP_H_
-#define PYWAVEMAP_MAP_H_
+#ifndef PYWAVEMAP_MAPS_H_
+#define PYWAVEMAP_MAPS_H_
 
 #include <nanobind/nanobind.h>
 
@@ -9,4 +9,4 @@ namespace wavemap {
 void add_map_bindings(nb::module_& m);
 }  // namespace wavemap
 
-#endif  // PYWAVEMAP_MAP_H_
+#endif  // PYWAVEMAP_MAPS_H_
diff --git a/library/python/src/indices.cc b/library/python/src/indices.cc
new file mode 100644
index 000000000..8c785cfb7
--- /dev/null
+++ b/library/python/src/indices.cc
@@ -0,0 +1,36 @@
+#include "pywavemap/indices.h"
+
+#include <nanobind/eigen/dense.h>
+#include <wavemap/core/indexing/ndtree_index.h>
+
+using namespace nb::literals;  // NOLINT
+
+namespace wavemap {
+void add_index_bindings(nb::module_& m) {
+  nb::class_<OctreeIndex>(m, "OctreeIndex",
+                          "A class representing indices of octree nodes.")
+      .def(nb::init<>())
+      .def(nb::init<OctreeIndex::Element, OctreeIndex::Position>(), "height"_a,
+           "position"_a)
+      .def_rw("height", &OctreeIndex::height, "height"_a = 0,
+              "The node's resolution level in the octree. A height of 0 "
+              "corresponds to the map’s maximum resolution. The node's size is "
+              "doubled each time the height is increased by 1.")
+      .def_rw("position", &OctreeIndex::position, "position"_a,
+              "The node's XYZ position in the octree’s grid at the resolution "
+              "level set by *height*.")
+      .def("computeParentIndex",
+           nb::overload_cast<>(&OctreeIndex::computeParentIndex, nb::const_),
+           "Compute the index of the node's direct parent.")
+      .def("computeParentIndex",
+           nb::overload_cast<OctreeIndex::Element>(
+               &OctreeIndex::computeParentIndex, nb::const_),
+           "parent_height"_a,
+           "Compute the index of the node's parent (or ancestor) at "
+           "*parent_height*.")
+      .def("computeChildIndex", &OctreeIndex::computeChildIndex,
+           "relative_child_index"_a,
+           "Compute the index of the node's n-th child, where n ranges from 0 "
+           "to 7.");
+}
+}  // namespace wavemap
diff --git a/library/python/src/map.cc b/library/python/src/map.cc
deleted file mode 100644
index 411f48fe5..000000000
--- a/library/python/src/map.cc
+++ /dev/null
@@ -1,81 +0,0 @@
-#include "pywavemap/map.h"
-
-#include <nanobind/eigen/dense.h>
-#include <nanobind/stl/filesystem.h>
-#include <nanobind/stl/shared_ptr.h>
-#include <wavemap/core/map/map_base.h>
-#include <wavemap/core/map/map_factory.h>
-#include <wavemap/io/file_conversions.h>
-
-using namespace nb::literals;  // NOLINT
-
-namespace wavemap {
-void add_map_bindings(nb::module_& m) {
-  nb::class_<MapBase>(m, "Map", "Base class for wavemap maps.")
-      .def_prop_ro("empty", &MapBase::empty, "Whether the map is empty.")
-      .def_prop_ro("size", &MapBase::size,
-                   "The number of cells or nodes in the map, for fixed or "
-                   "multi-resolution maps, respectively.")
-      .def("threshold", &MapBase::threshold,
-           "Threshold the occupancy values of all cells in the map to stay "
-           "within the range specified by its min_log_odds and max_log_odds.")
-      .def("prune", &MapBase::prune,
-           "Free up memory by pruning nodes that are no longer needed. Note "
-           "that this pruning operation is lossless and does not alter the "
-           "estimated occupancy posterior.")
-      .def("pruneSmart", &MapBase::pruneSmart,
-           "Similar to prune(), but avoids de-allocating nodes that were "
-           "recently updated and will likely be used again in the near future.")
-      .def("clear", &MapBase::clear, "Erase all cells in the map.")
-      .def_prop_ro("min_cell_width", &MapBase::getMinCellWidth,
-                   "Maximum map resolution, set as width of smallest cell it "
-                   "can represent.")
-      .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds,
-                   "Lower threshold for the occupancy values stored in the "
-                   "map, in log-odds.")
-      .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds,
-                   "Upper threshold for the occupancy values stored in the "
-                   "map, in log-odds.")
-      .def_prop_ro("memory_usage", &MapBase::getMemoryUsage,
-                   "The amount of memory used by the map, in bytes.")
-      .def_prop_ro("tree_height", &MapBase::getTreeHeight,
-                   "Height of the octree used to store the map. Note that this "
-                   "value is only defined for multi-resolution maps.")
-      .def_prop_ro(
-          "min_index", &MapBase::getMinIndex,
-          "Index of the minimum corner of the map's Axis Aligned Bounding Box.")
-      .def_prop_ro(
-          "max_index", &MapBase::getMaxIndex,
-          "Index of the maximum corner of the map's Axis Aligned Bounding Box.")
-      .def("getCellValue", &MapBase::getCellValue, "index"_a,
-           "Query the value of the map at a given index.")
-      .def("setCellValue", &MapBase::setCellValue, "index"_a,
-           "new_value"_a
-           "Set the value of the map at a given index.")
-      .def("addToCellValue", &MapBase::addToCellValue, "index"_a, "update"_a,
-           "Increment the value of the map at a given index.")
-      .def_static(
-          "create",
-          [](const param::Value& params) -> std::shared_ptr<MapBase> {
-            return MapFactory::create(params);
-          },
-          nb::sig("def create(parameters: dict) -> Map"), "parameters"_a,
-          "Create a new map based on the given settings.")
-      .def_static(
-          "load",
-          [](const std::filesystem::path& file_path)
-              -> std::shared_ptr<MapBase> {
-            std::shared_ptr<MapBase> map;
-            if (wavemap::io::fileToMap(file_path, map)) {
-              return map;
-            }
-            return nullptr;
-          },
-          "file_path"_a, "Load a wavemap map from a .wvmp file.")
-      .def(
-          "store",
-          [](const MapBase& self, const std::filesystem::path& file_path)
-              -> bool { return wavemap::io::mapToFile(self, file_path); },
-          "file_path"_a, "Store a wavemap map as a .wvmp file.");
-}
-}  // namespace wavemap
diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc
new file mode 100644
index 000000000..9a335b142
--- /dev/null
+++ b/library/python/src/maps.cc
@@ -0,0 +1,111 @@
+#include "pywavemap/maps.h"
+
+#include <nanobind/eigen/dense.h>
+#include <nanobind/stl/filesystem.h>
+#include <nanobind/stl/shared_ptr.h>
+#include <wavemap/core/map/hashed_chunked_wavelet_octree.h>
+#include <wavemap/core/map/hashed_wavelet_octree.h>
+#include <wavemap/core/map/map_base.h>
+#include <wavemap/core/map/map_factory.h>
+#include <wavemap/io/file_conversions.h>
+
+using namespace nb::literals;  // NOLINT
+
+namespace wavemap {
+void add_map_bindings(nb::module_& m) {
+  auto map_base =
+      nb::class_<MapBase>(m, "Map", "Base class for wavemap maps.")
+          .def_prop_ro("empty", &MapBase::empty, "Whether the map is empty.")
+          .def_prop_ro("size", &MapBase::size,
+                       "The number of cells or nodes in the map, for fixed or "
+                       "multi-resolution maps, respectively.")
+          .def("threshold", &MapBase::threshold,
+               "Threshold the occupancy values of all cells in the map to stay "
+               "within the range specified by its min_log_odds and "
+               "max_log_odds.")
+          .def(
+              "prune", &MapBase::prune,
+              "Free up memory by pruning nodes that are no longer needed. Note "
+              "that this pruning operation is lossless and does not alter the "
+              "estimated occupancy posterior.")
+          .def("pruneSmart", &MapBase::pruneSmart,
+               "Similar to prune(), but avoids de-allocating nodes that were "
+               "recently updated and will likely be used again in the near "
+               "future.")
+          .def("clear", &MapBase::clear, "Erase all cells in the map.")
+          .def_prop_ro(
+              "min_cell_width", &MapBase::getMinCellWidth,
+              "Maximum map resolution, set as width of smallest cell it "
+              "can represent.")
+          .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds,
+                       "Lower threshold for the occupancy values stored in the "
+                       "map, in log-odds.")
+          .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds,
+                       "Upper threshold for the occupancy values stored in the "
+                       "map, in log-odds.")
+          .def_prop_ro("memory_usage", &MapBase::getMemoryUsage,
+                       "The amount of memory used by the map, in bytes.")
+          .def_prop_ro(
+              "tree_height", &MapBase::getTreeHeight,
+              "Height of the octree used to store the map. Note that this "
+              "value is only defined for multi-resolution maps.")
+          .def_prop_ro("min_index", &MapBase::getMinIndex,
+                       "Index of the minimum corner of the map's Axis Aligned "
+                       "Bounding Box.")
+          .def_prop_ro("max_index", &MapBase::getMaxIndex,
+                       "Index of the maximum corner of the map's Axis Aligned "
+                       "Bounding Box.")
+          .def("getCellValue", &MapBase::getCellValue, "index"_a,
+               "Query the value of the map at a given index.")
+          .def("setCellValue", &MapBase::setCellValue, "index"_a,
+               "new_value"_a
+               "Set the value of the map at a given index.")
+          .def("addToCellValue", &MapBase::addToCellValue, "index"_a,
+               "update"_a, "Increment the value of the map at a given index.")
+          .def_static(
+              "create",
+              [](const param::Value& params) -> std::shared_ptr<MapBase> {
+                return MapFactory::create(params);
+              },
+              nb::sig("def create(parameters: dict) -> Map"), "parameters"_a,
+              "Create a new map based on the given settings.")
+          .def_static(
+              "load",
+              [](const std::filesystem::path& file_path)
+                  -> std::shared_ptr<MapBase> {
+                std::shared_ptr<MapBase> map;
+                if (wavemap::io::fileToMap(file_path, map)) {
+                  return map;
+                }
+                return nullptr;
+              },
+              "file_path"_a, "Load a wavemap map from a .wvmp file.")
+          .def(
+              "store",
+              [](const MapBase& self, const std::filesystem::path& file_path)
+                  -> bool { return wavemap::io::mapToFile(self, file_path); },
+              "file_path"_a, "Store a wavemap map as a .wvmp file.");
+
+  nb::class_<HashedWaveletOctree>(
+      m, "HashedWaveletOctree", map_base,
+      "A class that stores maps using hashed wavelet octrees.")
+      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+           "Query the value of the map at a given index.")
+      .def("getCellValue",
+           nb::overload_cast<const OctreeIndex&>(
+               &HashedWaveletOctree::getCellValue, nb::const_),
+           "node_index"_a,
+           "Query the value of the map at a given octree node index.");
+
+  nb::class_<HashedChunkedWaveletOctree>(
+      m, "HashedChunkedWaveletOctree", map_base,
+      "A class that stores maps using hashed chunked wavelet octrees.")
+      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+           "Query the value of the map at a given index.")
+      .def("getCellValue",
+           nb::overload_cast<const OctreeIndex&>(
+               &HashedChunkedWaveletOctree::getCellValue, nb::const_),
+           "node_index"_a,
+           "Query the value of the map at a given octree node index.");
+}
+}  // namespace wavemap
diff --git a/library/python/src/measurements.cc b/library/python/src/measurements.cc
index 93913d986..88db19beb 100644
--- a/library/python/src/measurements.cc
+++ b/library/python/src/measurements.cc
@@ -10,27 +10,32 @@ using namespace nb::literals;  // NOLINT
 namespace wavemap {
 void add_measurement_bindings(nb::module_& m) {
   // Poses
-  nb::class_<Rotation3D>(m, "Rotation")
+  nb::class_<Rotation3D>(m, "Rotation",
+                         "A class representing rotations in 3D space.")
       .def(nb::init<Rotation3D::RotationMatrix>(), "rotation_matrix"_a)
       .def("inverse", &Rotation3D::inverse, "Compute the rotation's inverse.");
-  nb::class_<Transformation3D>(m, "Pose")
+  nb::class_<Transformation3D>(m, "Pose",
+                               "A class representing poses in 3D space.")
       .def(nb::init<Rotation3D, Vector3D>(), "rotation"_a, "translation"_a)
       .def(nb::init<Transformation3D::TransformationMatrix>(),
            "transformation_matrix")
       .def("inverse", &Transformation3D::inverse,
-           "Compute the transformation's inverse.");
+           "Compute the pose's inverse.");
 
   // Pointclouds
-  nb::class_<Pointcloud<>>(m, "Pointcloud")
+  nb::class_<Pointcloud<>>(m, "Pointcloud", "A class to store pointclouds.")
       .def(nb::init<Pointcloud<>::Data>(), "point_matrix"_a);
-  nb::class_<PosedPointcloud<>>(m, "PosedPointcloud")
+  nb::class_<PosedPointcloud<>>(
+      m, "PosedPointcloud",
+      "A class to store pointclouds with an associated pose.")
       .def(nb::init<Transformation3D, Pointcloud<>>(), "pose"_a,
            "pointcloud"_a);
 
   // Images
-  nb::class_<Image<>>(m, "Image")
+  nb::class_<Image<>>(m, "Image", "A class to store depth images.")
       .def(nb::init<Image<>::Data>(), "pixel_matrix"_a);
-  nb::class_<PosedImage<>>(m, "PosedImage")
+  nb::class_<PosedImage<>>(
+      m, "PosedImage", "A class to store depth images with an associated pose.")
       .def(nb::init<Transformation3D, Image<>>(), "pose"_a, "image"_a);
 }
 }  // namespace wavemap
diff --git a/library/python/src/param.cc b/library/python/src/param.cc
index da1002288..f3c55f314 100644
--- a/library/python/src/param.cc
+++ b/library/python/src/param.cc
@@ -36,7 +36,7 @@ param::Array toParamArray(const nb::handle& py_value) {  // NOLINT
   param::Array array;
   array.reserve(nb::len(py_list));
   for (const auto& py_element : py_list) {  // NOLINT
-    array.template emplace_back(toParamValue(py_element));
+    array.emplace_back(toParamValue(py_element));
   }
   return array;
 }
diff --git a/library/python/src/pipeline.cc b/library/python/src/pipeline.cc
index ef8502219..9c81fbd1d 100644
--- a/library/python/src/pipeline.cc
+++ b/library/python/src/pipeline.cc
@@ -12,7 +12,7 @@ void add_pipeline_bindings(nb::module_& m) {
   nb::class_<Pipeline>(m, "Pipeline",
                        "A class to build pipelines of measurement integrators "
                        "and map operations.")
-      .def(nb::init<std::shared_ptr<MapBase>>())
+      .def(nb::init<std::shared_ptr<MapBase>>(), "map"_a)
       .def("clear", &Pipeline::clear,
            "Deregister all the pipeline's measurement integrators and map "
            "operations.")
diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc
index fb4739ac0..4bd76493d 100644
--- a/library/python/src/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -1,7 +1,8 @@
 #include <nanobind/nanobind.h>
 
+#include "pywavemap/indices.h"
 #include "pywavemap/logging.h"
-#include "pywavemap/map.h"
+#include "pywavemap/maps.h"
 #include "pywavemap/measurements.h"
 #include "pywavemap/param.h"
 #include "pywavemap/pipeline.h"
@@ -32,10 +33,13 @@ NB_MODULE(_cpp_bindings, m) {
                       "Submodule for wavemap's config system.");
   add_param_module(m_param);
 
+  // Bindings for index types
+  add_index_bindings(m);
+
   // Bindings for measurement types
   add_measurement_bindings(m);
 
-  // Bindings for wavemap maps
+  // Bindings for map types
   add_map_bindings(m);
 
   // Bindings for measurement integration and map update pipelines
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index 287ff7a1d..3f7170d42 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -1,3 +1,16 @@
-from ._cpp_bindings import __doc__  # Use module doc string as pkg doc string
-from ._cpp_bindings import (Map, Pipeline, Rotation, Pose, Pointcloud,
-                            PosedPointcloud, Image, PosedImage, logging, param)
+# Use module doc string as pkg doc string
+from ._cpp_bindings import __doc__
+
+# Binding types
+from ._cpp_bindings import OctreeIndex
+from ._cpp_bindings import (Rotation, Pose, Pointcloud, PosedPointcloud, Image,
+                            PosedImage)
+from ._cpp_bindings import (Map, HashedWaveletOctree,
+                            HashedChunkedWaveletOctree)
+from ._cpp_bindings import Pipeline
+
+# Binding submodules
+from ._cpp_bindings import logging, param
+
+# Regular modules
+from . import convert
diff --git a/library/python/src/pywavemap/convert.py b/library/python/src/pywavemap/convert.py
new file mode 100644
index 000000000..6de1706a3
--- /dev/null
+++ b/library/python/src/pywavemap/convert.py
@@ -0,0 +1,45 @@
+"""
+convert
+*******
+Submodule with common conversion functions for wavemap index types.
+"""
+
+import numpy as _np
+from ._cpp_bindings import OctreeIndex
+
+
+def cell_width_to_height(cell_width, min_cell_width):
+    """
+    Compute the minimum node height (resolution level) required to reach
+    a given width.
+    """
+    return int(_np.ceil(_np.log2(cell_width / min_cell_width)))
+
+
+def height_to_cell_width(min_cell_width, height):
+    """Compute the node width at a given height."""
+    return min_cell_width * float(_np.exp2(height))
+
+
+def scaled_point_to_nearest_index(point):
+    """Compute the nearest index to a point on the unit grid."""
+    return (point - 0.5).round().astype(int)
+
+
+def point_to_nearest_index(point, cell_width):
+    """
+    Compute the nearest index to a point on a grid with a given cell width.
+    """
+    return scaled_point_to_nearest_index(point / cell_width)
+
+
+def point_to_node_index(point, min_cell_width, height):
+    """
+    Compute the index of a node containing a given point.
+
+    :param min_cell_width: The grid resolution at height 0 (max map resolution).
+    :param height: The desired height (resolution level) of the node index.
+    """
+    node_width = height_to_cell_width(min_cell_width, height)
+    position_index = point_to_nearest_index(point, node_width)
+    return OctreeIndex(height, position_index)

From 5a6232cd1fd5511fb586bd0a6e5f8bc4ffdb9968 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 22 Aug 2024 13:32:10 +0200
Subject: [PATCH 37/85] Rename _cpp_bindings to _pywavemap_bindings s.t. name
 is self explaining

---
 library/python/CMakeLists.txt            | 18 +++++++++---------
 library/python/src/pywavemap.cc          |  2 +-
 library/python/src/pywavemap/__init__.py | 16 ++++++++--------
 library/python/src/pywavemap/convert.py  |  2 +-
 4 files changed, 19 insertions(+), 19 deletions(-)

diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 000ccf3f0..a7c1f7ec2 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -60,7 +60,7 @@ endif ()
 find_package(nanobind CONFIG REQUIRED)
 
 # Compile our extension
-nanobind_add_module(_cpp_bindings STABLE_ABI
+nanobind_add_module(_pywavemap_bindings STABLE_ABI
     src/pywavemap.cc
     src/indices.cc
     src/logging.cc
@@ -68,27 +68,27 @@ nanobind_add_module(_cpp_bindings STABLE_ABI
     src/measurements.cc
     src/param.cc
     src/pipeline.cc)
-set_wavemap_target_properties(_cpp_bindings)
-target_include_directories(_cpp_bindings PUBLIC include)
-target_link_libraries(_cpp_bindings PUBLIC
+set_wavemap_target_properties(_pywavemap_bindings)
+target_include_directories(_pywavemap_bindings PUBLIC include)
+target_link_libraries(_pywavemap_bindings PUBLIC
     wavemap::wavemap_core wavemap::wavemap_io wavemap::wavemap_pipeline)
 # Disable some default wavemap warnings that trigger on nanobind
-target_compile_options(_cpp_bindings PRIVATE
+target_compile_options(_pywavemap_bindings PRIVATE
     -Wno-pedantic -Wno-unused-result -Wno-suggest-attribute=const)
 
 # Install directive for scikit-build-core
-install(TARGETS _cpp_bindings LIBRARY DESTINATION pywavemap)
+install(TARGETS _pywavemap_bindings LIBRARY DESTINATION pywavemap)
 
 # Generate stubs
 nanobind_add_stub(pywavemap_stub INSTALL_TIME
-    MODULE _cpp_bindings
+    MODULE _pywavemap_bindings
     OUTPUT "pywavemap/__init__.pyi"
     PYTHON_PATH "pywavemap")
 nanobind_add_stub(pywavemap_logging_stub INSTALL_TIME
-    MODULE _cpp_bindings.logging
+    MODULE _pywavemap_bindings.logging
     OUTPUT "pywavemap/logging.pyi"
     PYTHON_PATH "pywavemap")
 nanobind_add_stub(pywavemap_param_stub INSTALL_TIME
-    MODULE _cpp_bindings.param
+    MODULE _pywavemap_bindings.param
     OUTPUT "pywavemap/param.pyi"
     PYTHON_PATH "pywavemap")
diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc
index 4bd76493d..d02c17655 100644
--- a/library/python/src/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -10,7 +10,7 @@
 using namespace wavemap;  // NOLINT
 namespace nb = nanobind;
 
-NB_MODULE(_cpp_bindings, m) {
+NB_MODULE(_pywavemap_bindings, m) {
   m.doc() =
       "pywavemap\n"
       "*********\n"
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index 3f7170d42..f7e2018f8 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -1,16 +1,16 @@
 # Use module doc string as pkg doc string
-from ._cpp_bindings import __doc__
+from ._pywavemap_bindings import __doc__
 
 # Binding types
-from ._cpp_bindings import OctreeIndex
-from ._cpp_bindings import (Rotation, Pose, Pointcloud, PosedPointcloud, Image,
-                            PosedImage)
-from ._cpp_bindings import (Map, HashedWaveletOctree,
-                            HashedChunkedWaveletOctree)
-from ._cpp_bindings import Pipeline
+from ._pywavemap_bindings import OctreeIndex
+from ._pywavemap_bindings import (Rotation, Pose, Pointcloud, PosedPointcloud,
+                                  Image, PosedImage)
+from ._pywavemap_bindings import (Map, HashedWaveletOctree,
+                                  HashedChunkedWaveletOctree)
+from ._pywavemap_bindings import Pipeline
 
 # Binding submodules
-from ._cpp_bindings import logging, param
+from ._pywavemap_bindings import logging, param
 
 # Regular modules
 from . import convert
diff --git a/library/python/src/pywavemap/convert.py b/library/python/src/pywavemap/convert.py
index 6de1706a3..9a7849fe1 100644
--- a/library/python/src/pywavemap/convert.py
+++ b/library/python/src/pywavemap/convert.py
@@ -5,7 +5,7 @@
 """
 
 import numpy as _np
-from ._cpp_bindings import OctreeIndex
+from ._pywavemap_bindings import OctreeIndex
 
 
 def cell_width_to_height(cell_width, min_cell_width):

From 5c91283bff1673f516294e560663b63ea7d01540 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Fri, 23 Aug 2024 14:48:36 +0200
Subject: [PATCH 38/85] Extend and improve documentation and examples

---
 docs/pages/parameters/map_operations.rst      |   2 +
 docs/pages/tutorials/cpp.rst                  |  29 +++--
 docs/pages/tutorials/python.rst               |  55 ++++++---
 docs/pages/tutorials/ros1.rst                 |  18 ++-
 examples/cpp/io/CMakeLists.txt                |  10 ++
 examples/cpp/io/load_map_from_file.cc         |   3 +-
 examples/cpp/io/load_map_from_stream.cc       |  14 +++
 examples/cpp/io/save_map_to_file.cc           |   2 +-
 examples/cpp/io/save_map_to_stream.cc         |  17 +++
 examples/python/io/load_map_from_file.py      |   4 +-
 examples/python/io/load_params_from_file.py   |  29 +++++
 examples/python/io/save_map_to_file.py        |   4 +-
 examples/python/mapping/full_pipeline.py      |  18 +--
 examples/python/panoptic_mapping.py           |  12 +-
 examples/python/queries/_dummy_objects.py     |   6 +-
 examples/python/queries/multi_resolution.py   |  10 +-
 .../include/wavemap/io/stream_conversions.h   |   8 +-
 library/cpp/src/io/stream_conversions.cc      | 106 +++++++++++++++---
 18 files changed, 277 insertions(+), 70 deletions(-)
 create mode 100644 examples/cpp/io/load_map_from_stream.cc
 create mode 100644 examples/cpp/io/save_map_to_stream.cc
 create mode 100644 examples/python/io/load_params_from_file.py

diff --git a/docs/pages/parameters/map_operations.rst b/docs/pages/parameters/map_operations.rst
index 1dd054179..01caebd55 100644
--- a/docs/pages/parameters/map_operations.rst
+++ b/docs/pages/parameters/map_operations.rst
@@ -28,6 +28,8 @@ This operation frees up memory by pruning octree nodes that are no longer needed
     :project: wavemap_cpp
     :members:
 
+.. _configuration_map_operations_ros1:
+
 ROS1 Interface
 **************
 In addition to the map operations provided by the C++ library, wavemap's ROS1 interface offers several ROS-specific operations.
diff --git a/docs/pages/tutorials/cpp.rst b/docs/pages/tutorials/cpp.rst
index 91fb68a9f..a2e33a073 100644
--- a/docs/pages/tutorials/cpp.rst
+++ b/docs/pages/tutorials/cpp.rst
@@ -38,24 +38,39 @@ We **strongly recommend** to also call the ``set_wavemap_target_properties`` fun
 
 Code examples
 *************
-
 In the following sections, you'll find sample code for common tasks. If you'd like to request examples for additional tasks or contribute new examples, please don't hesitate to `contact us <https://github.com/ethz-asl/wavemap/issues>`_.
 
-Serialization
-=============
+Serializing maps
+================
+In this section, we'll demonstrate how to serialize and deserialize maps using wavemap's lightweight and efficient binary format. This format is consistent across wavemap's C++, Python, and ROS interfaces. For instance, you can create maps on a robot with ROS and later load them into a rendering engine plugin that only depends on wavemap's C++ library.
 
-Files
------
-Saving maps to files:
+Binary files
+------------
+Maps can be saved to disk with
 
 .. literalinclude:: ../../../examples/cpp/io/save_map_to_file.cc
     :language: c++
 
-Loading maps from files:
+.. _cpp-code-examples-read-map:
+
+and read using
 
 .. literalinclude:: ../../../examples/cpp/io/load_map_from_file.cc
     :language: c++
 
+Byte streams
+------------
+We also provide an alternative, lower-level interface to convert maps to (byte) streams
+
+.. literalinclude:: ../../../examples/cpp/io/save_map_to_stream.cc
+    :language: c++
+
+and read them with
+
+.. literalinclude:: ../../../examples/cpp/io/load_map_from_stream.cc
+    :language: c++
+
+
 Queries
 =======
 In this section, we illustrate how you can query the map and classify whether a point or region of interest is occupied.
diff --git a/docs/pages/tutorials/python.rst b/docs/pages/tutorials/python.rst
index d0c399ebe..44bc91a8d 100644
--- a/docs/pages/tutorials/python.rst
+++ b/docs/pages/tutorials/python.rst
@@ -15,7 +15,7 @@ Before you start, make sure you :doc:`installed pywavemap <../installation/pytho
 
 In your python files, you can then load the API by simply calling::
 
-    import pywavemap
+    import pywavemap as wave
 
 Code examples
 *************
@@ -26,24 +26,58 @@ In the following sections, we provide sample code for common tasks. If you'd lik
 
     All of the examples scripts that follow can be found :gh_file:`here <examples/python>`.
 
+Mapping
+=======
+The only requirements to build wavemap maps are that you have a set of
+
+1. depth measurements,
+2. sensor pose (estimates) for each measurement.
+
+We usually use depth measurements from depth cameras or 3D LiDARs, but any source would work as long as a corresponding :ref:`projection <configuration_projection_models>` and :ref:`measurement <configuration_measurement_models>` model is available. To help you get started quickly, we provide example configs for various sensor setups :gh_file:`here <interfaces/ros1/wavemap_ros/config>`. An overview of all the available settings is provided on the :doc:`parameters page <../parameters/index>`.
+
+Example pipeline
+----------------
+
+.. literalinclude:: ../../../examples/python/mapping/full_pipeline.py
+    :language: python
+
 Serialization
 =============
+Next, we show how you can serialize and deserialize common wavemap objects, for example to save and load them from files.
 
-Files
------
-Saving maps to files:
+Maps
+----
+Wavemap uses a lightweight, efficient binary format to serialize its maps. The same format is used across wavemap's C++, Python and ROS interfaces. You could therefore, for example, create maps on a robot with ROS and subsequently analyze them in Python.
+
+Binary files
+^^^^^^^^^^^^
+Maps can be saved to disk using
 
 .. literalinclude:: ../../../examples/python/io/save_map_to_file.py
     :language: python
 
-Loading maps from files:
+.. _python-code-examples-read-map:
+
+and read with
 
 .. literalinclude:: ../../../examples/python/io/load_map_from_file.py
     :language: python
 
+Configs
+-------
+In the previous mapping pipeline example, the configuration parameters for the map and the measurement integration components were hard-coded. To make your setup more flexible, you can use configuration files. We will demonstrate how to work with YAML files, which is the format we use for wavemap's :gh_file:`example configs <interfaces/ros1/wavemap_ros/config>`. However, pywavemap is flexible and can support any parameter format that can be read into a Python `dict`.
+
+
+YAML files
+^^^^^^^^^^
+
+.. literalinclude:: ../../../examples/python/io/load_params_from_file.py
+    :language: python
+
+
 Queries
 =======
-In this section, we illustrate how you can query the map and classify whether a point or region of interest is occupied.
+In this section, we show how you can query wavemap maps and classify whether a point or region of interest is occupied.
 
 Node indices
 ------------
@@ -98,12 +132,3 @@ Once the estimated occupancy at a node or point has been retrieved, it can be cl
 
 .. literalinclude:: ../../../examples/python/queries/classification.py
     :language: python
-
-Mapping
-=======
-
-Full pipeline
--------------
-
-.. literalinclude:: ../../../examples/python/mapping/full_pipeline.py
-    :language: python
diff --git a/docs/pages/tutorials/ros1.rst b/docs/pages/tutorials/ros1.rst
index 588f67afc..e03e76062 100644
--- a/docs/pages/tutorials/ros1.rst
+++ b/docs/pages/tutorials/ros1.rst
@@ -26,6 +26,20 @@ We usually use depth measurements from depth cameras or 3D LiDARs, but any sourc
 
 To help you get started quickly, we provide example :gh_file:`config <interfaces/ros1/wavemap_ros/config>` and ROS :gh_file:`launch <interfaces/ros1/wavemap_ros/launch>` files for various sensor setups and use cases. An overview of all the available settings is provided on the :doc:`parameters page <../parameters/index>`.
 
+Publishing maps
+===============
+Wavemap's ROS server offers multiple ways to publish its maps to ROS topics, enabling visualization and usage by other ROS nodes. Please refer to the documentation on :ref:`ROS1 map operations <configuration_map_operations_ros1>` for an overview of the available options.
+
+Saving maps
+===========
+The server's map can also be written to disk by calling its ``save_map`` service as follows:
+
+.. code-block:: bash
+
+    rosservice call /wavemap/save_map "file_path: '/path/to/your/map.wvmp'"
+
+Saved maps can subsequently be used :ref:`in C++ <cpp-code-examples-read-map>` (with or without ROS) and :ref:`in Python <python-code-examples-read-map>`.
+
 Your own code
 *************
 We now briefly discuss how to set up your own ROS1 package to use wavemap, before proceeding to code examples.
@@ -88,12 +102,12 @@ Code examples
 =============
 Since wavemap's ROS1 interface extends its C++ API, all of the :ref:`C++ API's code examples <cpp-code-examples>` can directly be used in ROS.
 
-The only code required to receive maps over a ROS topic in your own ROS node is:
+Additionally, the following code can be used to receive maps over a ROS topic
 
 .. literalinclude:: ../../../examples/ros1/io/receive_map_over_ros.cc
     :language: c++
 
-To send a map, the following code can be used:
+and maps can be sent over ROS with
 
 .. literalinclude:: ../../../examples/ros1/io/send_map_over_ros.cc
     :language: c++
diff --git a/examples/cpp/io/CMakeLists.txt b/examples/cpp/io/CMakeLists.txt
index 2537509b7..21446f226 100644
--- a/examples/cpp/io/CMakeLists.txt
+++ b/examples/cpp/io/CMakeLists.txt
@@ -8,3 +8,13 @@ add_executable(load_map_from_file load_map_from_file.cc)
 set_wavemap_target_properties(load_map_from_file)
 target_link_libraries(load_map_from_file PUBLIC
     wavemap::wavemap_core wavemap::wavemap_io)
+
+add_executable(save_map_to_stream save_map_to_stream.cc)
+set_wavemap_target_properties(save_map_to_stream)
+target_link_libraries(save_map_to_stream PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io)
+
+add_executable(load_map_from_stream load_map_from_stream.cc)
+set_wavemap_target_properties(load_map_from_stream)
+target_link_libraries(load_map_from_stream PUBLIC
+    wavemap::wavemap_core wavemap::wavemap_io)
diff --git a/examples/cpp/io/load_map_from_file.cc b/examples/cpp/io/load_map_from_file.cc
index b26e951cb..5862575ae 100644
--- a/examples/cpp/io/load_map_from_file.cc
+++ b/examples/cpp/io/load_map_from_file.cc
@@ -5,5 +5,6 @@ int main(int, char**) {
   wavemap::MapBase::Ptr loaded_map;
 
   // Load the map
-  wavemap::io::fileToMap("/some/path/to/your/map.wvmp", loaded_map);
+  const bool success =
+      wavemap::io::fileToMap("/path/to/your/map.wvmp", loaded_map);
 }
diff --git a/examples/cpp/io/load_map_from_stream.cc b/examples/cpp/io/load_map_from_stream.cc
new file mode 100644
index 000000000..1f855230b
--- /dev/null
+++ b/examples/cpp/io/load_map_from_stream.cc
@@ -0,0 +1,14 @@
+#include <strstream>
+
+#include <wavemap/io/stream_conversions.h>
+
+int main(int, char**) {
+  // Create a smart pointer that will own the loaded map
+  wavemap::MapBase::Ptr loaded_map;
+
+  // Create an input stream for illustration purposes
+  std::istrstream input_stream{""};
+
+  // Load the map
+  const bool success = wavemap::io::streamToMap(input_stream, loaded_map);
+}
diff --git a/examples/cpp/io/save_map_to_file.cc b/examples/cpp/io/save_map_to_file.cc
index a686cec90..fa7c862b5 100644
--- a/examples/cpp/io/save_map_to_file.cc
+++ b/examples/cpp/io/save_map_to_file.cc
@@ -6,5 +6,5 @@ int main(int, char**) {
   wavemap::HashedWaveletOctree map(config);
 
   // Save the map
-  wavemap::io::mapToFile(map, "/some/path/to/your/map.wvmp");
+  const bool success = wavemap::io::mapToFile(map, "/path/to/your/map.wvmp");
 }
diff --git a/examples/cpp/io/save_map_to_stream.cc b/examples/cpp/io/save_map_to_stream.cc
new file mode 100644
index 000000000..251beec78
--- /dev/null
+++ b/examples/cpp/io/save_map_to_stream.cc
@@ -0,0 +1,17 @@
+#include <strstream>
+
+#include <wavemap/io/stream_conversions.h>
+
+int main(int, char**) {
+  // Create an empty map for illustration purposes
+  wavemap::HashedWaveletOctreeConfig config;
+  wavemap::HashedWaveletOctree map(config);
+
+  // Create an output stream for illustration purposes
+  std::ostrstream output_stream;
+
+  // Save the map
+  bool success = wavemap::io::mapToStream(map, output_stream);
+  output_stream.flush();
+  success &= output_stream.good();
+}
diff --git a/examples/python/io/load_map_from_file.py b/examples/python/io/load_map_from_file.py
index 20d414ec4..fdd903e96 100644
--- a/examples/python/io/load_map_from_file.py
+++ b/examples/python/io/load_map_from_file.py
@@ -1,7 +1,7 @@
 import os
-import pywavemap as pw
+import pywavemap as wave
 
 # Load the map
 user_home = os.path.expanduser('~')
 map_path = os.path.join(user_home, "your_map.wvmp")
-your_map = pw.Map.load(map_path)
+your_map = wave.Map.load(map_path)
diff --git a/examples/python/io/load_params_from_file.py b/examples/python/io/load_params_from_file.py
new file mode 100644
index 000000000..0ae24fd65
--- /dev/null
+++ b/examples/python/io/load_params_from_file.py
@@ -0,0 +1,29 @@
+import os
+import yaml
+import pywavemap as wave
+
+
+def create_map_from_config(config_file_path):
+    """
+    Example function that creates a map based on parameters in a YAML file.
+    """
+    with open(config_file_path) as file:
+        try:
+            config = yaml.safe_load(file)
+        except yaml.YAMLError as exc:
+            print(exc)
+            return None
+
+    if isinstance(config, dict) and "map" in config.keys():
+        return wave.Map.create(config["map"])
+
+    return None
+
+
+# Provide the path to your config
+config_dir = os.path.abspath(
+    os.path.join(__file__, "../../../../interfaces/ros1/wavemap_ros/config"))
+config_file = os.path.join(config_dir, "wavemap_panoptic_mapping_rgbd.yaml")
+
+# Create the map
+your_map = create_map_from_config(config_file)
diff --git a/examples/python/io/save_map_to_file.py b/examples/python/io/save_map_to_file.py
index 309d757c8..30a85cbdf 100644
--- a/examples/python/io/save_map_to_file.py
+++ b/examples/python/io/save_map_to_file.py
@@ -1,8 +1,8 @@
 import os
-import pywavemap as pw
+import pywavemap as wave
 
 # Create an empty map for illustration purposes
-your_map = pw.Map.create({
+your_map = wave.Map.create({
     "type": "hashed_chunked_wavelet_octree",
     "min_cell_width": {
         "meters": 0.1
diff --git a/examples/python/mapping/full_pipeline.py b/examples/python/mapping/full_pipeline.py
index 46600f349..788fc0edf 100644
--- a/examples/python/mapping/full_pipeline.py
+++ b/examples/python/mapping/full_pipeline.py
@@ -2,7 +2,7 @@
 import csv
 from PIL import Image as PilImage
 import numpy as np
-import pywavemap as pw
+import pywavemap as wave
 
 # Parameters
 home_dir = os.path.expanduser('~')
@@ -11,7 +11,7 @@
 output_map_path = os.path.join(home_dir, "your_map.wvmp")
 
 # Create a map
-your_map = pw.Map.create({
+your_map = wave.Map.create({
     "type": "hashed_chunked_wavelet_octree",
     "min_cell_width": {
         "meters": 0.05
@@ -19,7 +19,7 @@
 })
 
 # Create a measurement integration pipeline
-pipeline = pw.Pipeline(your_map)
+pipeline = wave.Pipeline(your_map)
 # Add map operations
 pipeline.addOperation({
     "type": "threshold_map",
@@ -27,7 +27,6 @@
         "seconds": 5.0
     }
 })
-pipeline.addOperation({"type": "prune_map", "once_every": {"seconds": 10.0}})
 # Add a measurement integrator
 pipeline.addIntegrator(
     "my_integrator", {
@@ -89,7 +88,7 @@
         current_index += 1
         raise SystemExit
     cv_img = PilImage.open(depth_file)
-    image = pw.Image(np.array(cv_img).transpose())
+    image = wave.Image(np.array(cv_img).transpose())
 
     # Load transform
     pose_file = file_path_prefix + "_pose.txt"
@@ -104,16 +103,19 @@
             for row in range(4):
                 for col in range(4):
                     transform[row, col] = pose_data[row * 4 + col]
-    pose = pw.Pose(transform)
+    pose = wave.Pose(transform)
 
     # Integrate the depth image
     print(f"Integrating measurement {ids[current_index]}")
-    pipeline.runPipeline(["my_integrator"], pw.PosedImage(pose, image))
+    pipeline.runPipeline(["my_integrator"], wave.PosedImage(pose, image))
 
     current_index += 1
 
+# Remove map nodes that are no longer needed
+your_map.prune()
+
 # Save the map
-print(f"Saving map of size {your_map.memory_usage}")
+print(f"Saving map of size {your_map.memory_usage} bytes")
 your_map.store(output_map_path)
 
 # Avoids leak warnings on old Python versions with lazy garbage collectors
diff --git a/examples/python/panoptic_mapping.py b/examples/python/panoptic_mapping.py
index 1640e7a64..5c5c70d49 100644
--- a/examples/python/panoptic_mapping.py
+++ b/examples/python/panoptic_mapping.py
@@ -5,7 +5,7 @@
 from PIL import Image as PilImage
 import numpy as np
 import yaml
-import pywavemap as pw
+import pywavemap as wave
 from tqdm import tqdm
 
 
@@ -14,9 +14,9 @@ class DataLoader():
     def __init__(self, params, data_path):
         self.data_path = data_path
 
-        self.map = pw.Map.create(params["map"])
+        self.map = wave.Map.create(params["map"])
 
-        self.pipeline = pw.Pipeline(self.map)
+        self.pipeline = wave.Pipeline(self.map)
 
         for operation in params["map_operations"]:
             self.pipeline.addOperation(operation)
@@ -74,7 +74,7 @@ def integrate_frame(self):
 
         # Load depth image
         cv_img = PilImage.open(depth_file)
-        image = pw.Image(np.array(cv_img).transpose())
+        image = wave.Image(np.array(cv_img).transpose())
 
         # Load transform
         if os.path.isfile(pose_file):
@@ -84,10 +84,10 @@ def integrate_frame(self):
                 for row in range(4):
                     for col in range(4):
                         transform[row, col] = pose_data[row * 4 + col]
-        pose = pw.Pose(transform)
+        pose = wave.Pose(transform)
 
         self.pipeline.runPipeline([self.integrator_name],
-                                  pw.PosedImage(pose, image))
+                                  wave.PosedImage(pose, image))
 
         self.current_index += 1
 
diff --git a/examples/python/queries/_dummy_objects.py b/examples/python/queries/_dummy_objects.py
index 9f88e05e9..756c0b207 100644
--- a/examples/python/queries/_dummy_objects.py
+++ b/examples/python/queries/_dummy_objects.py
@@ -1,12 +1,14 @@
-import pywavemap as pw
+import pywavemap as wave
 
 
 def example_occupancy_log_odds():
+    """Function that returns a dummy occupancy value to be used in examples."""
     return 0.0
 
 
 def example_map():
-    return pw.Map.create({
+    """Function that returns a dummy map to be used in examples."""
+    return wave.Map.create({
         "type": "hashed_chunked_wavelet_octree",
         "min_cell_width": {
             "meters": 0.1
diff --git a/examples/python/queries/multi_resolution.py b/examples/python/queries/multi_resolution.py
index 0713d6804..750d70f64 100644
--- a/examples/python/queries/multi_resolution.py
+++ b/examples/python/queries/multi_resolution.py
@@ -1,5 +1,5 @@
 import numpy as np
-import pywavemap as pw
+import pywavemap as wave
 import _dummy_objects
 
 # Load a map
@@ -11,10 +11,10 @@
 
 # Convert it to an octree node index
 map_min_cell_width = your_map.min_cell_width
-query_height = pw.convert.cell_width_to_height(query_min_cell_width,
-                                               map_min_cell_width)
-query_index = pw.convert.point_to_node_index(query_point, map_min_cell_width,
-                                             query_height)
+query_height = wave.convert.cell_width_to_height(query_min_cell_width,
+                                                 map_min_cell_width)
+query_index = wave.convert.point_to_node_index(query_point, map_min_cell_width,
+                                               query_height)
 
 # Query the map
 occupancy_log_odds = your_map.getCellValue(query_index)
diff --git a/library/cpp/include/wavemap/io/stream_conversions.h b/library/cpp/include/wavemap/io/stream_conversions.h
index 47758522d..f3341999b 100644
--- a/library/cpp/include/wavemap/io/stream_conversions.h
+++ b/library/cpp/include/wavemap/io/stream_conversions.h
@@ -16,16 +16,16 @@ namespace wavemap::io {
 bool mapToStream(const MapBase& map, std::ostream& ostream);
 bool streamToMap(std::istream& istream, MapBase::Ptr& map);
 
-void mapToStream(const HashedBlocks& map, std::ostream& ostream);
+bool mapToStream(const HashedBlocks& map, std::ostream& ostream);
 bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map);
 
-void mapToStream(const WaveletOctree& map, std::ostream& ostream);
+bool mapToStream(const WaveletOctree& map, std::ostream& ostream);
 bool streamToMap(std::istream& istream, WaveletOctree::Ptr& map);
 
-void mapToStream(const HashedWaveletOctree& map, std::ostream& ostream);
+bool mapToStream(const HashedWaveletOctree& map, std::ostream& ostream);
 bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map);
 
-void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream);
+bool mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream);
 }  // namespace wavemap::io
 
 #endif  // WAVEMAP_IO_STREAM_CONVERSIONS_H_
diff --git a/library/cpp/src/io/stream_conversions.cc b/library/cpp/src/io/stream_conversions.cc
index 4edeab938..6384a9ffc 100644
--- a/library/cpp/src/io/stream_conversions.cc
+++ b/library/cpp/src/io/stream_conversions.cc
@@ -5,25 +5,21 @@ bool mapToStream(const MapBase& map, std::ostream& ostream) {
   // Call the appropriate mapToStream converter based on the map's derived type
   if (const auto* hashed_blocks = dynamic_cast<const HashedBlocks*>(&map);
       hashed_blocks) {
-    io::mapToStream(*hashed_blocks, ostream);
-    return true;
+    return io::mapToStream(*hashed_blocks, ostream);
   }
   if (const auto* wavelet_octree = dynamic_cast<const WaveletOctree*>(&map);
       wavelet_octree) {
-    io::mapToStream(*wavelet_octree, ostream);
-    return true;
+    return io::mapToStream(*wavelet_octree, ostream);
   }
   if (const auto* hashed_wavelet_octree =
           dynamic_cast<const HashedWaveletOctree*>(&map);
       hashed_wavelet_octree) {
-    io::mapToStream(*hashed_wavelet_octree, ostream);
-    return true;
+    return io::mapToStream(*hashed_wavelet_octree, ostream);
   }
   if (const auto* hashed_chunked_wavelet_octree =
           dynamic_cast<const HashedChunkedWaveletOctree*>(&map);
       hashed_chunked_wavelet_octree) {
-    io::mapToStream(*hashed_chunked_wavelet_octree, ostream);
-    return true;
+    return io::mapToStream(*hashed_chunked_wavelet_octree, ostream);
   }
 
   LOG(WARNING) << "Could not serialize requested map to stream. "
@@ -32,6 +28,11 @@ bool mapToStream(const MapBase& map, std::ostream& ostream) {
 }
 
 bool streamToMap(std::istream& istream, MapBase::Ptr& map) {
+  // Check if the input stream can be read from
+  if (!istream.good()) {
+    return false;
+  }
+
   // Call the appropriate streamToMap converter based on the received map's type
   const auto storage_format = streamable::StorageFormat::peek(istream);
   switch (storage_format) {
@@ -68,7 +69,12 @@ bool streamToMap(std::istream& istream, MapBase::Ptr& map) {
   }
 }
 
-void mapToStream(const HashedBlocks& map, std::ostream& ostream) {
+bool mapToStream(const HashedBlocks& map, std::ostream& ostream) {
+  // Check if the output stream can be written to
+  if (!ostream.good()) {
+    return false;
+  }
+
   // Serialize the map's data structure type
   streamable::StorageFormat storage_format =
       streamable::StorageFormat::kHashedBlocks;
@@ -85,6 +91,11 @@ void mapToStream(const HashedBlocks& map, std::ostream& ostream) {
   // Iterate over all the map's blocks
   map.forEachBlock(
       [&ostream](const Index3D& block_index, const HashedBlocks::Block& block) {
+        // Stop if any writing errors occurred
+        if (!ostream.good()) {
+          return;
+        }
+
         // Serialize the block's metadata
         streamable::HashedBlockHeader block_header;
         block_header.block_offset = {block_index.x(), block_index.y(),
@@ -98,9 +109,17 @@ void mapToStream(const HashedBlocks& map, std::ostream& ostream) {
                         sizeof(value_serialized));
         }
       });
+
+  // Return true if no write errors occurred
+  return ostream.good();
 }
 
 bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map) {
+  // Check if the input stream can be read from
+  if (!istream.good()) {
+    return false;
+  }
+
   // Make sure the map in the input stream is of the correct type
   if (streamable::StorageFormat::read(istream) !=
       streamable::StorageFormat::kHashedBlocks) {
@@ -119,6 +138,11 @@ bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map) {
   // Deserialize all the blocks
   for (size_t block_idx = 0; block_idx < hashed_blocks_header.num_blocks;
        ++block_idx) {
+    // Stop if any reading errors occurred
+    if (!istream.good()) {
+      return false;
+    }
+
     // Deserialize the block header, containing its position
     const auto block_header = streamable::HashedBlockHeader::read(istream);
     const Index3D block_index{block_header.block_offset.x,
@@ -136,10 +160,16 @@ bool streamToMap(std::istream& istream, HashedBlocks::Ptr& map) {
     }
   }
 
-  return true;
+  // Return true if no read errors occurred
+  return istream.good();
 }
 
-void mapToStream(const WaveletOctree& map, std::ostream& ostream) {
+bool mapToStream(const WaveletOctree& map, std::ostream& ostream) {
+  // Check if the output stream can be written to
+  if (!ostream.good()) {
+    return false;
+  }
+
   // Serialize the map's data structure type
   streamable::StorageFormat storage_format =
       streamable::StorageFormat::kWaveletOctree;
@@ -171,9 +201,17 @@ void mapToStream(const WaveletOctree& map, std::ostream& ostream) {
     }
     streamable_node.write(ostream);
   }
+
+  // Return true if no write errors occurred
+  return ostream.good();
 }
 
 bool streamToMap(std::istream& istream, WaveletOctree::Ptr& map) {
+  // Check if the input stream can be read from
+  if (!istream.good()) {
+    return false;
+  }
+
   // Make sure the map in the input stream is of the correct type
   if (streamable::StorageFormat::read(istream) !=
       streamable::StorageFormat::kWaveletOctree) {
@@ -220,10 +258,16 @@ bool streamToMap(std::istream& istream, WaveletOctree::Ptr& map) {
     }
   }
 
-  return true;
+  // Return true if no read errors occurred
+  return istream.good();
 }
 
-void mapToStream(const HashedWaveletOctree& map, std::ostream& ostream) {
+bool mapToStream(const HashedWaveletOctree& map, std::ostream& ostream) {
+  // Check if the output stream can be written to
+  if (!ostream.good()) {
+    return false;
+  }
+
   // Define convenience types and constants
   struct StackElement {
     const FloatingPoint scale;
@@ -250,6 +294,11 @@ void mapToStream(const HashedWaveletOctree& map, std::ostream& ostream) {
   // Iterate over all the map's blocks
   map.forEachBlock([&ostream, min_log_odds, max_log_odds](
                        const Index3D& block_index, const auto& block) {
+    // Stop if any writing errors occurred
+    if (!ostream.good()) {
+      return;
+    }
+
     // Serialize the block's metadata
     streamable::HashedWaveletOctreeBlockHeader block_header;
     block_header.root_node_offset = {block_index.x(), block_index.y(),
@@ -295,9 +344,17 @@ void mapToStream(const HashedWaveletOctree& map, std::ostream& ostream) {
       streamable_node.write(ostream);
     }
   });
+
+  // Return true if no write errors occurred
+  return ostream.good();
 }
 
 bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map) {
+  // Check if the input stream can be read from
+  if (!istream.good()) {
+    return false;
+  }
+
   // Make sure the map in the input stream is of the correct type
   if (streamable::StorageFormat::read(istream) !=
       streamable::StorageFormat::kHashedWaveletOctree) {
@@ -317,6 +374,11 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map) {
   // Deserialize all the blocks
   for (size_t block_idx = 0;
        block_idx < hashed_wavelet_octree_header.num_blocks; ++block_idx) {
+    // Stop if any reading errors occurred
+    if (!istream.good()) {
+      return false;
+    }
+
     // Deserialize the block header, containing its position and scale coeff.
     const auto block_header =
         streamable::HashedWaveletOctreeBlockHeader::read(istream);
@@ -353,10 +415,16 @@ bool streamToMap(std::istream& istream, HashedWaveletOctree::Ptr& map) {
     }
   }
 
-  return true;
+  // Return true if no read errors occurred
+  return istream.good();
 }
 
-void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) {
+bool mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) {
+  // Check if the output stream can be written to
+  if (!ostream.good()) {
+    return false;
+  }
+
   // Define convenience types and constants
   struct StackElement {
     const FloatingPoint scale;
@@ -383,6 +451,11 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) {
   // Iterate over all the map's blocks
   map.forEachBlock([&ostream, min_log_odds, max_log_odds](
                        const Index3D& block_index, const auto& block) {
+    // Stop if any writing errors occurred
+    if (!ostream.good()) {
+      return;
+    }
+
     // Serialize the block's metadata
     streamable::HashedWaveletOctreeBlockHeader block_header;
     block_header.root_node_offset = {block_index.x(), block_index.y(),
@@ -427,5 +500,8 @@ void mapToStream(const HashedChunkedWaveletOctree& map, std::ostream& ostream) {
       streamable_node.write(ostream);
     }
   });
+
+  // Return true if no write errors occurred
+  return ostream.good();
 }
 }  // namespace wavemap::io

From 4401c577db6229802ff8cd4e7a5510843bd05789 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Fri, 23 Aug 2024 17:15:41 +0200
Subject: [PATCH 39/85] Add bindings for accelerated queries

---
 docs/python_api/index.rst                     |  3 ++
 examples/python/queries/_dummy_objects.py     |  2 +-
 .../python/queries/accelerated_queries.py     | 31 +++++++++++-
 .../queries/nearest_neighbor_interpolation.py |  3 +-
 .../python/queries/trilinear_interpolation.py |  3 +-
 .../cpp/include/wavemap/core/map/map_base.h   |  6 +--
 .../core/utils/query/query_accelerator.h      | 13 ++++-
 library/python/src/maps.cc                    | 47 +++++++++++++++++++
 library/python/src/pywavemap/__init__.py      |  1 +
 9 files changed, 100 insertions(+), 9 deletions(-)

diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst
index 892765a97..19cf78e86 100644
--- a/docs/python_api/index.rst
+++ b/docs/python_api/index.rst
@@ -15,6 +15,9 @@ Python API
     :show-inheritance:
     :members:
 
+.. autoclass:: pywavemap.HashedWaveletOctreeQueryAccelerator
+    :members:
+
 .. autoclass:: pywavemap.OctreeIndex
     :members:
 
diff --git a/examples/python/queries/_dummy_objects.py b/examples/python/queries/_dummy_objects.py
index 756c0b207..7060f9b30 100644
--- a/examples/python/queries/_dummy_objects.py
+++ b/examples/python/queries/_dummy_objects.py
@@ -9,7 +9,7 @@ def example_occupancy_log_odds():
 def example_map():
     """Function that returns a dummy map to be used in examples."""
     return wave.Map.create({
-        "type": "hashed_chunked_wavelet_octree",
+        "type": "hashed_wavelet_octree",
         "min_cell_width": {
             "meters": 0.1
         }
diff --git a/examples/python/queries/accelerated_queries.py b/examples/python/queries/accelerated_queries.py
index a515c072c..3c4c49709 100644
--- a/examples/python/queries/accelerated_queries.py
+++ b/examples/python/queries/accelerated_queries.py
@@ -1,2 +1,29 @@
-# TODO(victorr): Extend the bindings
-# TODO(victorr): Include vectorized version
+import numpy as np
+import pywavemap as wave
+
+import _dummy_objects
+
+# Load a map
+your_map = _dummy_objects.example_map()
+
+# Create a query accelerator
+accelerator = wave.HashedWaveletOctreeQueryAccelerator(your_map)
+
+# Query fixed resolution indices one by one
+value_1 = accelerator.getCellValue(np.array([1, 2, 3]))
+value_2 = accelerator.getCellValue(np.array([4, 5, 6]))
+value_3 = accelerator.getCellValue(np.array([7, 8, 9]))
+
+# Query node indices one by one
+value_4 = accelerator.getCellValue(wave.OctreeIndex(0, np.array([1, 2, 3])))
+value_5 = accelerator.getCellValue(wave.OctreeIndex(1, np.array([4, 5, 6])))
+value_6 = accelerator.getCellValue(wave.OctreeIndex(2, np.array([7, 8, 9])))
+
+# Vectorized query for a list of fixed-resolution indices
+indices = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
+values = accelerator.getCellValues(indices)
+
+# Vectorized query for a list of node indices
+node_heights = np.random.randint(0, 6, size=(64 * 64 * 32, 1))
+node_indices = np.concatenate((node_heights, indices), axis=1)
+node_values = accelerator.getCellValues(node_indices)
diff --git a/examples/python/queries/nearest_neighbor_interpolation.py b/examples/python/queries/nearest_neighbor_interpolation.py
index 483367aad..5416498d9 100644
--- a/examples/python/queries/nearest_neighbor_interpolation.py
+++ b/examples/python/queries/nearest_neighbor_interpolation.py
@@ -1,7 +1,8 @@
 import numpy as np
+import _dummy_objects
 
 # Load a map
-# from io_load_map_from_file import your_map
+your_map = _dummy_objects.example_map()
 
 # Declare the point to query [in map frame]
 query_point = np.array([0.4, .5, 0.6])
diff --git a/examples/python/queries/trilinear_interpolation.py b/examples/python/queries/trilinear_interpolation.py
index 866c8e06b..30d06fc8f 100644
--- a/examples/python/queries/trilinear_interpolation.py
+++ b/examples/python/queries/trilinear_interpolation.py
@@ -1,7 +1,8 @@
 import numpy as np
+import _dummy_objects
 
 # Load a map
-# from io_load_map_from_file import your_map
+your_map = _dummy_objects.example_map()
 
 # Declare the point to query [in map frame]
 query_point = np.array([0.4, .5, 0.6])
diff --git a/library/cpp/include/wavemap/core/map/map_base.h b/library/cpp/include/wavemap/core/map/map_base.h
index fa8611011..31b054e49 100644
--- a/library/cpp/include/wavemap/core/map/map_base.h
+++ b/library/cpp/include/wavemap/core/map/map_base.h
@@ -32,12 +32,12 @@ struct MapType : TypeSelector<MapType> {
  */
 struct MapBaseConfig : ConfigBase<MapBaseConfig, 3> {
   //! Maximum resolution of the map, set as the width of the smallest cell that
-  //! it can represent.
+  //! it can represent
   Meters<FloatingPoint> min_cell_width = 0.1f;
 
-  //! Lower threshold for the occupancy values stored in the map, in log-odds.
+  //! Lower threshold for the occupancy values stored in the map, in log-odds
   FloatingPoint min_log_odds = -2.f;
-  //! Upper threshold for the occupancy values stored in the map, in log-odds.
+  //! Upper threshold for the occupancy values stored in the map, in log-odds
   FloatingPoint max_log_odds = 4.f;
 
   static MemberMap memberMap;
diff --git a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
index 50cf09481..44e28139c 100644
--- a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
+++ b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
@@ -106,7 +106,11 @@ class QueryAccelerator<NdtreeBlockHash<CellDataT, dim>> {
   std::array<NodeType*, morton::kMaxTreeHeight<dim>> node_stack{};
 };
 
-// Query accelerator for hashed wavelet octrees
+/**
+ * A class that accelerates queries by caching block and parent node  addresses
+ * to speed up data structure traversals, and intermediate wavelet decompression
+ * results to reduce redundant computation.
+ */
 template <>
 class QueryAccelerator<HashedWaveletOctree> {
  public:
@@ -114,12 +118,19 @@ class QueryAccelerator<HashedWaveletOctree> {
 
   explicit QueryAccelerator(const HashedWaveletOctree& map) : map_(map) {}
 
+  //! Reset the cache
+  //! @note This method must be called whenever the map changes, not only to
+  //!       guarantee correct values (after node value changes) but also to
+  //!       avoid segmentation fault after map topology changes (e.g. after
+  //!       pruning).
   void reset();
 
+  //! Query the value of the map at a given index
   FloatingPoint getCellValue(const Index3D& index) {
     return getCellValue(OctreeIndex{0, index});
   }
 
+  //! Query the value of the map at a given octree node index
   FloatingPoint getCellValue(const OctreeIndex& index);
 
  private:
diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc
index 9a335b142..c328a5a04 100644
--- a/library/python/src/maps.cc
+++ b/library/python/src/maps.cc
@@ -7,6 +7,7 @@
 #include <wavemap/core/map/hashed_wavelet_octree.h>
 #include <wavemap/core/map/map_base.h>
 #include <wavemap/core/map/map_factory.h>
+#include <wavemap/core/utils/query/query_accelerator.h>
 #include <wavemap/io/file_conversions.h>
 
 using namespace nb::literals;  // NOLINT
@@ -107,5 +108,51 @@ void add_map_bindings(nb::module_& m) {
                &HashedChunkedWaveletOctree::getCellValue, nb::const_),
            "node_index"_a,
            "Query the value of the map at a given octree node index.");
+
+  nb::class_<QueryAccelerator<HashedWaveletOctree>>(
+      m, "HashedWaveletOctreeQueryAccelerator",
+      "A class that accelerates queries by caching block and parent node "
+      "addresses to speed up data structure traversals, and intermediate "
+      "wavelet decompression results to reduce redundant computation.")
+      .def(nb::init<const HashedWaveletOctree&>(), "map"_a)
+      .def("getCellValue",
+           nb::overload_cast<const Index3D&>(
+               &QueryAccelerator<HashedWaveletOctree>::getCellValue),
+           "index"_a, "Query the value of the map at a given index.")
+      .def("getCellValue",
+           nb::overload_cast<const OctreeIndex&>(
+               &QueryAccelerator<HashedWaveletOctree>::getCellValue),
+           "node_index"_a,
+           "Query the value of the map at a given octree node index.")
+      .def(
+          "getCellValues",
+          [](QueryAccelerator<HashedWaveletOctree>& self,
+             const Eigen::Matrix<IndexElement, Eigen::Dynamic, 3>& indices) {
+            const auto num_queries = static_cast<int>(indices.rows());
+            Eigen::VectorXf results{num_queries};
+            for (int query_idx = 0; query_idx < num_queries; ++query_idx) {
+              results[query_idx] = self.getCellValue(indices.row(query_idx));
+            }
+            return results;
+          },
+          "index_list"_a,
+          "Query the map at the given indices, provided as a matrix with one "
+          "(x, y, z) index per row.")
+      .def(
+          "getCellValues",
+          [](QueryAccelerator<HashedWaveletOctree>& self,
+             const Eigen::Matrix<IndexElement, Eigen::Dynamic, 4>& indices) {
+            const auto num_queries = static_cast<int>(indices.rows());
+            Eigen::VectorXf results{num_queries};
+            for (int query_idx = 0; query_idx < num_queries; ++query_idx) {
+              const auto& row = indices.row(query_idx);
+              OctreeIndex node_index{row[0], row.tail<3>()};
+              results[query_idx] = self.getCellValue(node_index);
+            }
+            return results;
+          },
+          "node_index_list"_a,
+          "Query the map at the given node indices, provided as a matrix with "
+          "one (height, x, y, z) node index per row.");
 }
 }  // namespace wavemap
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index f7e2018f8..303b3206b 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -7,6 +7,7 @@
                                   Image, PosedImage)
 from ._pywavemap_bindings import (Map, HashedWaveletOctree,
                                   HashedChunkedWaveletOctree)
+from ._pywavemap_bindings import HashedWaveletOctreeQueryAccelerator
 from ._pywavemap_bindings import Pipeline
 
 # Binding submodules

From ae96e5855f611dcf83cd77210e0520549043b94c Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Fri, 23 Aug 2024 18:01:42 +0200
Subject: [PATCH 40/85] Use nanobind instead of Eigen arrays to avoid type cast
 overhead

---
 library/python/src/maps.cc | 52 ++++++++++++++++++++++++++++----------
 1 file changed, 39 insertions(+), 13 deletions(-)

diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc
index c328a5a04..1bebcd335 100644
--- a/library/python/src/maps.cc
+++ b/library/python/src/maps.cc
@@ -127,13 +127,26 @@ void add_map_bindings(nb::module_& m) {
       .def(
           "getCellValues",
           [](QueryAccelerator<HashedWaveletOctree>& self,
-             const Eigen::Matrix<IndexElement, Eigen::Dynamic, 3>& indices) {
-            const auto num_queries = static_cast<int>(indices.rows());
-            Eigen::VectorXf results{num_queries};
-            for (int query_idx = 0; query_idx < num_queries; ++query_idx) {
-              results[query_idx] = self.getCellValue(indices.row(query_idx));
+             const nb::ndarray<IndexElement, nb::shape<-1, 3>, nb::device::cpu>&
+                 indices) {
+            // Create nb::ndarray view for efficient access
+            const auto index_view = indices.view();
+            const auto num_queries = index_view.shape(0);
+            // Allocate and populate raw results array
+            auto* results = new float[num_queries];
+            for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
+              results[query_idx] = self.getCellValue(
+                  {index_view(query_idx, 0), index_view(query_idx, 1),
+                   index_view(query_idx, 2)});
             }
-            return results;
+            // Create Python capsule that deallocates the results array when
+            // all references to it expire
+            nb::capsule owner(results, [](void* p) noexcept {
+              delete[] reinterpret_cast<float*>(p);
+            });
+            // Return results as numpy array
+            return nb::ndarray<nb::numpy, float>{
+                results, {num_queries, 1u}, owner};
           },
           "index_list"_a,
           "Query the map at the given indices, provided as a matrix with one "
@@ -141,15 +154,28 @@ void add_map_bindings(nb::module_& m) {
       .def(
           "getCellValues",
           [](QueryAccelerator<HashedWaveletOctree>& self,
-             const Eigen::Matrix<IndexElement, Eigen::Dynamic, 4>& indices) {
-            const auto num_queries = static_cast<int>(indices.rows());
-            Eigen::VectorXf results{num_queries};
-            for (int query_idx = 0; query_idx < num_queries; ++query_idx) {
-              const auto& row = indices.row(query_idx);
-              OctreeIndex node_index{row[0], row.tail<3>()};
+             const nb::ndarray<IndexElement, nb::shape<-1, 4>, nb::device::cpu>&
+                 indices) {
+            // Create nb::ndarray view for efficient access
+            auto index_view = indices.view();
+            const auto num_queries = index_view.shape(0);
+            // Allocate and populate raw results array
+            auto* results = new float[num_queries];
+            for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
+              const OctreeIndex node_index{
+                  index_view(query_idx, 0),
+                  {index_view(query_idx, 1), index_view(query_idx, 2),
+                   index_view(query_idx, 3)}};
               results[query_idx] = self.getCellValue(node_index);
             }
-            return results;
+            // Create Python capsule that deallocates the results array when
+            // all references to it expire
+            nb::capsule owner(results, [](void* p) noexcept {
+              delete[] reinterpret_cast<float*>(p);
+            });
+            // Return results as numpy array
+            return nb::ndarray<nb::numpy, float>{
+                results, {num_queries, 1u}, owner};
           },
           "node_index_list"_a,
           "Query the map at the given node indices, provided as a matrix with "

From ed813106b3ed9089f16f4fe8288578c6bd621383 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 27 Aug 2024 14:51:29 +0200
Subject: [PATCH 41/85] Bind wavemap's index conversion functions instead of
 rewriting in Python

---
 docs/python_api/index.rst                  |  4 ++
 library/python/CMakeLists.txt              |  5 +++
 library/python/include/pywavemap/convert.h | 12 ++++++
 library/python/src/convert.cc              | 47 ++++++++++++++++++++++
 library/python/src/pywavemap.cc            |  9 +++++
 library/python/src/pywavemap/__init__.py   |  5 +--
 library/python/src/pywavemap/convert.py    | 45 ---------------------
 7 files changed, 78 insertions(+), 49 deletions(-)
 create mode 100644 library/python/include/pywavemap/convert.h
 create mode 100644 library/python/src/convert.cc
 delete mode 100644 library/python/src/pywavemap/convert.py

diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst
index 19cf78e86..6dd72f95b 100644
--- a/docs/python_api/index.rst
+++ b/docs/python_api/index.rst
@@ -41,6 +41,10 @@ Python API
 
 .. automodule:: pywavemap.convert
     :members:
+.. automethod:: pywavemap.convert.cell_width_to_height
+.. automethod:: pywavemap.convert.height_to_cell_width
+.. automethod:: pywavemap.convert.point_to_nearest_index
+.. automethod:: pywavemap.convert.point_to_node_index
 
 .. automodule:: pywavemap.logging
 .. automethod:: pywavemap.logging.set_level
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index a7c1f7ec2..7b8f62330 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -62,6 +62,7 @@ find_package(nanobind CONFIG REQUIRED)
 # Compile our extension
 nanobind_add_module(_pywavemap_bindings STABLE_ABI
     src/pywavemap.cc
+    src/convert.cc
     src/indices.cc
     src/logging.cc
     src/maps.cc
@@ -84,6 +85,10 @@ nanobind_add_stub(pywavemap_stub INSTALL_TIME
     MODULE _pywavemap_bindings
     OUTPUT "pywavemap/__init__.pyi"
     PYTHON_PATH "pywavemap")
+nanobind_add_stub(pywavemap_convert_stub INSTALL_TIME
+    MODULE _pywavemap_bindings.convert
+    OUTPUT "pywavemap/convert.pyi"
+    PYTHON_PATH "pywavemap")
 nanobind_add_stub(pywavemap_logging_stub INSTALL_TIME
     MODULE _pywavemap_bindings.logging
     OUTPUT "pywavemap/logging.pyi"
diff --git a/library/python/include/pywavemap/convert.h b/library/python/include/pywavemap/convert.h
new file mode 100644
index 000000000..2f8e76932
--- /dev/null
+++ b/library/python/include/pywavemap/convert.h
@@ -0,0 +1,12 @@
+#ifndef PYWAVEMAP_CONVERT_H_
+#define PYWAVEMAP_CONVERT_H_
+
+#include <nanobind/nanobind.h>
+
+namespace nb = nanobind;
+
+namespace wavemap {
+void add_convert_module(nb::module_& m_convert);
+}  // namespace wavemap
+
+#endif  // PYWAVEMAP_CONVERT_H_
diff --git a/library/python/src/convert.cc b/library/python/src/convert.cc
new file mode 100644
index 000000000..424aad96a
--- /dev/null
+++ b/library/python/src/convert.cc
@@ -0,0 +1,47 @@
+#include "pywavemap/convert.h"
+
+#include <nanobind/eigen/dense.h>
+#include <wavemap/core/indexing/index_conversions.h>
+
+using namespace nb::literals;  // NOLINT
+
+namespace wavemap {
+void add_convert_module(nb::module_& m_convert) {
+  m_convert.def(
+      "cell_width_to_height",
+      [](FloatingPoint cell_width, FloatingPoint min_cell_width) {
+        return convert::cellWidthToHeight(cell_width, 1.f / min_cell_width);
+      },
+      "cell_width"_a, "min_cell_width"_a,
+      "Compute the minimum node height (resolution level) required to reach"
+      "a given node width.\n\n"
+      "    :param cell_width: The desired node width.\n"
+      "    :param min_cell_width: The grid resolution at height 0 "
+      "(max map resolution).");
+
+  m_convert.def("height_to_cell_width", &convert::heightToCellWidth,
+                "min_cell_width"_a, "height"_a,
+                "Compute the node width at a given height.\n\n"
+                "    :param min_cell_width: The grid resolution at height 0 "
+                "(max map resolution).\n"
+                "    :param height: The desired height (resolution level) of "
+                "the node index.");
+
+  m_convert.def(
+      "point_to_nearest_index",
+      [](const Point3D& point, FloatingPoint cell_width) {
+        return convert::pointToNearestIndex<3>(point, 1.f / cell_width);
+      },
+      "point"_a, "cell_width"_a,
+      "Compute the nearest index to a point on a grid with a given "
+      "cell width.");
+
+  m_convert.def("point_to_node_index", &convert::pointToNodeIndex<3>, "point"_a,
+                "min_cell_width"_a, "height"_a,
+                "Compute the index of a node containing a given point.\n\n"
+                "    :param min_cell_width: The grid resolution at height 0 "
+                "(max map resolution).\n"
+                "    :param height: The desired height (resolution level) of "
+                "the node index.");
+}
+}  // namespace wavemap
diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc
index d02c17655..88cff63dd 100644
--- a/library/python/src/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -1,5 +1,6 @@
 #include <nanobind/nanobind.h>
 
+#include "pywavemap/convert.h"
 #include "pywavemap/indices.h"
 #include "pywavemap/logging.h"
 #include "pywavemap/maps.h"
@@ -33,6 +34,14 @@ NB_MODULE(_pywavemap_bindings, m) {
                       "Submodule for wavemap's config system.");
   add_param_module(m_param);
 
+  // Bindings for wavemap's index conversion functions
+  nb::module_ m_convert = m.def_submodule(
+      "convert",
+      "convert\n"
+      "=====\n"
+      "Submodule with common conversion functions for wavemap index types.");
+  add_convert_module(m_convert);
+
   // Bindings for index types
   add_index_bindings(m);
 
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index 303b3206b..b6577e9e9 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -11,7 +11,4 @@
 from ._pywavemap_bindings import Pipeline
 
 # Binding submodules
-from ._pywavemap_bindings import logging, param
-
-# Regular modules
-from . import convert
+from ._pywavemap_bindings import logging, param, convert
diff --git a/library/python/src/pywavemap/convert.py b/library/python/src/pywavemap/convert.py
deleted file mode 100644
index 9a7849fe1..000000000
--- a/library/python/src/pywavemap/convert.py
+++ /dev/null
@@ -1,45 +0,0 @@
-"""
-convert
-*******
-Submodule with common conversion functions for wavemap index types.
-"""
-
-import numpy as _np
-from ._pywavemap_bindings import OctreeIndex
-
-
-def cell_width_to_height(cell_width, min_cell_width):
-    """
-    Compute the minimum node height (resolution level) required to reach
-    a given width.
-    """
-    return int(_np.ceil(_np.log2(cell_width / min_cell_width)))
-
-
-def height_to_cell_width(min_cell_width, height):
-    """Compute the node width at a given height."""
-    return min_cell_width * float(_np.exp2(height))
-
-
-def scaled_point_to_nearest_index(point):
-    """Compute the nearest index to a point on the unit grid."""
-    return (point - 0.5).round().astype(int)
-
-
-def point_to_nearest_index(point, cell_width):
-    """
-    Compute the nearest index to a point on a grid with a given cell width.
-    """
-    return scaled_point_to_nearest_index(point / cell_width)
-
-
-def point_to_node_index(point, min_cell_width, height):
-    """
-    Compute the index of a node containing a given point.
-
-    :param min_cell_width: The grid resolution at height 0 (max map resolution).
-    :param height: The desired height (resolution level) of the node index.
-    """
-    node_width = height_to_cell_width(min_cell_width, height)
-    position_index = point_to_nearest_index(point, node_width)
-    return OctreeIndex(height, position_index)

From 25eb5f91ff3557b04d73f80df5d7e969c740c607 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 27 Aug 2024 18:00:45 +0200
Subject: [PATCH 42/85] Improve consistency between chunked and regular octree
 map

---
 .../wavemap/core/map/hashed_chunked_wavelet_octree_block.h | 1 +
 .../src/core/map/hashed_chunked_wavelet_octree_block.cc    | 7 +++++++
 library/cpp/src/core/map/hashed_wavelet_octree_block.cc    | 2 +-
 3 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h
index 3e885eff7..5f114afbc 100644
--- a/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h
+++ b/library/cpp/include/wavemap/core/map/hashed_chunked_wavelet_octree_block.h
@@ -30,6 +30,7 @@ class HashedChunkedWaveletOctreeBlock {
   size_t size() const { return chunked_ndtree_.size(); }
   void threshold();
   void prune();
+  void clear();
 
   FloatingPoint getCellValue(const OctreeIndex& index) const;
   void setCellValue(const OctreeIndex& index, FloatingPoint new_value);
diff --git a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc
index 329eba90a..164ee7d49 100644
--- a/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc
+++ b/library/cpp/src/core/map/hashed_chunked_wavelet_octree_block.cc
@@ -22,6 +22,13 @@ void HashedChunkedWaveletOctreeBlock::prune() {
   }
 }
 
+void HashedChunkedWaveletOctreeBlock::clear() {
+  ProfilerZoneScoped;
+  root_scale_coefficient_ = Coefficients::Scale{};
+  chunked_ndtree_.clear();
+  setLastUpdatedStamp();
+}
+
 void HashedChunkedWaveletOctreeBlock::setCellValue(const OctreeIndex& index,
                                                    FloatingPoint new_value) {
   setNeedsPruning();
diff --git a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc
index 92359f2c5..a96c40c2d 100644
--- a/library/cpp/src/core/map/hashed_wavelet_octree_block.cc
+++ b/library/cpp/src/core/map/hashed_wavelet_octree_block.cc
@@ -25,7 +25,7 @@ void HashedWaveletOctreeBlock::clear() {
   ProfilerZoneScoped;
   root_scale_coefficient_ = Coefficients::Scale{};
   ndtree_.clear();
-  last_updated_stamp_ = Time::now();
+  setLastUpdatedStamp();
 }
 
 void HashedWaveletOctreeBlock::setCellValue(const OctreeIndex& index,

From c78a6751d5d562999eadb85268a02a4d3af40757 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 27 Aug 2024 18:41:32 +0200
Subject: [PATCH 43/85] Fix issues with links to Sphinx refs scrolling too far

Add custom CSS for span elements (default Sphinx refs) to match our theme's general scroll margin.
---
 docs/_static/custom.css         | 4 ++++
 library/python/src/pywavemap.cc | 2 +-
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/docs/_static/custom.css b/docs/_static/custom.css
index b559aaaf4..9630190d8 100644
--- a/docs/_static/custom.css
+++ b/docs/_static/custom.css
@@ -3,3 +3,7 @@
     line-height: 1.75rem;
     margin-top: 1.5rem
 }
+
+#content span {
+    scroll-margin: 5rem;
+}
diff --git a/library/python/src/pywavemap.cc b/library/python/src/pywavemap.cc
index 88cff63dd..2cc7ba0ae 100644
--- a/library/python/src/pywavemap.cc
+++ b/library/python/src/pywavemap.cc
@@ -38,7 +38,7 @@ NB_MODULE(_pywavemap_bindings, m) {
   nb::module_ m_convert = m.def_submodule(
       "convert",
       "convert\n"
-      "=====\n"
+      "=======\n"
       "Submodule with common conversion functions for wavemap index types.");
   add_convert_module(m_convert);
 

From f3ae0302ee54dbf55f7087607d899afab8635832 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 28 Aug 2024 13:00:40 +0200
Subject: [PATCH 44/85] Add custom 404 page to documentation site

---
 .github/workflows/cd.yml |  2 +-
 .github/workflows/ci.yml |  2 +-
 docs/404.rst             | 14 ++++++++++++++
 docs/conf.py             |  7 +++++--
 4 files changed, 21 insertions(+), 4 deletions(-)
 create mode 100644 docs/404.rst

diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml
index 57da91546..226b1fe37 100644
--- a/.github/workflows/cd.yml
+++ b/.github/workflows/cd.yml
@@ -144,7 +144,7 @@ jobs:
           apt-get update
           apt-get install -q -y --no-install-recommends python3-pip doxygen
           apt-get install -q -y --no-install-recommends latexmk texlive-latex-extra tex-gyre texlive-fonts-recommended texlive-latex-recommended
-          pip3 install exhale sphinx-sitemap sphinx-design
+          pip3 install exhale sphinx-sitemap sphinx-design sphinx-notfound-page
           pip3 install sphinxawesome-theme --pre
           pip3 install "sphinx<7,>6"
 
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index f7cea6035..66d30d185 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -150,7 +150,7 @@ jobs:
           apt-get update
           apt-get install -q -y --no-install-recommends python3-pip doxygen
           apt-get install -q -y --no-install-recommends latexmk texlive-latex-extra tex-gyre texlive-fonts-recommended texlive-latex-recommended
-          pip3 install exhale sphinx-sitemap sphinx-design
+          pip3 install exhale sphinx-sitemap sphinx-design sphinx-notfound-page
           pip3 install sphinxawesome-theme --pre
           pip3 install "sphinx<7,>6"
 
diff --git a/docs/404.rst b/docs/404.rst
new file mode 100644
index 000000000..4e390c996
--- /dev/null
+++ b/docs/404.rst
@@ -0,0 +1,14 @@
+---
+permalink: /404.html
+---
+
+:orphan:
+
+404 - Page Not Found
+####################
+
+Oops! The page you're looking for doesn't exist.
+
+It might have been moved, or the URL could be incorrect. Try using the sidebar or search function to find what you need.
+
+Need help? Feel free to `contact us <https://github.com/ethz-asl/wavemap/issues/new/choose>`_ for questions or to report an issue.
diff --git a/docs/conf.py b/docs/conf.py
index 00e428771..2f9e12e28 100644
--- a/docs/conf.py
+++ b/docs/conf.py
@@ -19,8 +19,8 @@
 # General configuration
 extensions = [
     'sphinx.ext.mathjax', "sphinx.ext.extlinks", 'sphinx.ext.githubpages',
-    'sphinx.ext.autodoc', 'sphinx_design', 'sphinx_sitemap', 'breathe',
-    'exhale'
+    'sphinx.ext.autodoc', 'sphinx_design', 'sphinx_sitemap',
+    'notfound.extension', 'breathe', 'exhale'
 ]
 templates_path = ['_templates']
 source_suffix = ['.rst', '.md']
@@ -71,6 +71,9 @@
 # Sitemap specific options
 sitemap_url_scheme = "{link}"
 
+# Options for the 404 page
+notfound_urls_prefix = "/wavemap/"
+
 # Options for LaTeX output
 latex_elements = {}
 latex_documents = [

From e6660118725a7725a986351a709ea0af51c24e73 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 28 Aug 2024 14:16:04 +0200
Subject: [PATCH 45/85] Use query accelerator by default without exposing it to
 users

---
 .../python/queries/accelerated_queries.py     | 18 +------
 library/python/src/maps.cc                    | 50 +++++++------------
 library/python/src/pywavemap/__init__.py      |  1 -
 3 files changed, 21 insertions(+), 48 deletions(-)

diff --git a/examples/python/queries/accelerated_queries.py b/examples/python/queries/accelerated_queries.py
index 3c4c49709..c8ea87b64 100644
--- a/examples/python/queries/accelerated_queries.py
+++ b/examples/python/queries/accelerated_queries.py
@@ -1,29 +1,15 @@
 import numpy as np
-import pywavemap as wave
 
 import _dummy_objects
 
 # Load a map
 your_map = _dummy_objects.example_map()
 
-# Create a query accelerator
-accelerator = wave.HashedWaveletOctreeQueryAccelerator(your_map)
-
-# Query fixed resolution indices one by one
-value_1 = accelerator.getCellValue(np.array([1, 2, 3]))
-value_2 = accelerator.getCellValue(np.array([4, 5, 6]))
-value_3 = accelerator.getCellValue(np.array([7, 8, 9]))
-
-# Query node indices one by one
-value_4 = accelerator.getCellValue(wave.OctreeIndex(0, np.array([1, 2, 3])))
-value_5 = accelerator.getCellValue(wave.OctreeIndex(1, np.array([4, 5, 6])))
-value_6 = accelerator.getCellValue(wave.OctreeIndex(2, np.array([7, 8, 9])))
-
 # Vectorized query for a list of fixed-resolution indices
 indices = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
-values = accelerator.getCellValues(indices)
+values = your_map.getCellValues(indices)
 
 # Vectorized query for a list of node indices
 node_heights = np.random.randint(0, 6, size=(64 * 64 * 32, 1))
 node_indices = np.concatenate((node_heights, indices), axis=1)
-node_values = accelerator.getCellValues(node_indices)
+node_values = your_map.getCellValues(node_indices)
diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc
index 1bebcd335..a4a43152a 100644
--- a/library/python/src/maps.cc
+++ b/library/python/src/maps.cc
@@ -96,46 +96,21 @@ void add_map_bindings(nb::module_& m) {
            nb::overload_cast<const OctreeIndex&>(
                &HashedWaveletOctree::getCellValue, nb::const_),
            "node_index"_a,
-           "Query the value of the map at a given octree node index.");
-
-  nb::class_<HashedChunkedWaveletOctree>(
-      m, "HashedChunkedWaveletOctree", map_base,
-      "A class that stores maps using hashed chunked wavelet octrees.")
-      .def("getCellValue", &MapBase::getCellValue, "index"_a,
-           "Query the value of the map at a given index.")
-      .def("getCellValue",
-           nb::overload_cast<const OctreeIndex&>(
-               &HashedChunkedWaveletOctree::getCellValue, nb::const_),
-           "node_index"_a,
-           "Query the value of the map at a given octree node index.");
-
-  nb::class_<QueryAccelerator<HashedWaveletOctree>>(
-      m, "HashedWaveletOctreeQueryAccelerator",
-      "A class that accelerates queries by caching block and parent node "
-      "addresses to speed up data structure traversals, and intermediate "
-      "wavelet decompression results to reduce redundant computation.")
-      .def(nb::init<const HashedWaveletOctree&>(), "map"_a)
-      .def("getCellValue",
-           nb::overload_cast<const Index3D&>(
-               &QueryAccelerator<HashedWaveletOctree>::getCellValue),
-           "index"_a, "Query the value of the map at a given index.")
-      .def("getCellValue",
-           nb::overload_cast<const OctreeIndex&>(
-               &QueryAccelerator<HashedWaveletOctree>::getCellValue),
-           "node_index"_a,
            "Query the value of the map at a given octree node index.")
       .def(
           "getCellValues",
-          [](QueryAccelerator<HashedWaveletOctree>& self,
+          [](const HashedWaveletOctree& self,
              const nb::ndarray<IndexElement, nb::shape<-1, 3>, nb::device::cpu>&
                  indices) {
+            // Create a query accelerator
+            QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
             // Create nb::ndarray view for efficient access
             const auto index_view = indices.view();
             const auto num_queries = index_view.shape(0);
             // Allocate and populate raw results array
             auto* results = new float[num_queries];
             for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
-              results[query_idx] = self.getCellValue(
+              results[query_idx] = query_accelerator.getCellValue(
                   {index_view(query_idx, 0), index_view(query_idx, 1),
                    index_view(query_idx, 2)});
             }
@@ -153,9 +128,11 @@ void add_map_bindings(nb::module_& m) {
           "(x, y, z) index per row.")
       .def(
           "getCellValues",
-          [](QueryAccelerator<HashedWaveletOctree>& self,
+          [](const HashedWaveletOctree& self,
              const nb::ndarray<IndexElement, nb::shape<-1, 4>, nb::device::cpu>&
                  indices) {
+            // Create a query accelerator
+            QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
             // Create nb::ndarray view for efficient access
             auto index_view = indices.view();
             const auto num_queries = index_view.shape(0);
@@ -166,7 +143,7 @@ void add_map_bindings(nb::module_& m) {
                   index_view(query_idx, 0),
                   {index_view(query_idx, 1), index_view(query_idx, 2),
                    index_view(query_idx, 3)}};
-              results[query_idx] = self.getCellValue(node_index);
+              results[query_idx] = query_accelerator.getCellValue(node_index);
             }
             // Create Python capsule that deallocates the results array when
             // all references to it expire
@@ -180,5 +157,16 @@ void add_map_bindings(nb::module_& m) {
           "node_index_list"_a,
           "Query the map at the given node indices, provided as a matrix with "
           "one (height, x, y, z) node index per row.");
+
+  nb::class_<HashedChunkedWaveletOctree>(
+      m, "HashedChunkedWaveletOctree", map_base,
+      "A class that stores maps using hashed chunked wavelet octrees.")
+      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+           "Query the value of the map at a given index.")
+      .def("getCellValue",
+           nb::overload_cast<const OctreeIndex&>(
+               &HashedChunkedWaveletOctree::getCellValue, nb::const_),
+           "node_index"_a,
+           "Query the value of the map at a given octree node index.");
 }
 }  // namespace wavemap
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index b6577e9e9..fd40eb74f 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -7,7 +7,6 @@
                                   Image, PosedImage)
 from ._pywavemap_bindings import (Map, HashedWaveletOctree,
                                   HashedChunkedWaveletOctree)
-from ._pywavemap_bindings import HashedWaveletOctreeQueryAccelerator
 from ._pywavemap_bindings import Pipeline
 
 # Binding submodules

From f67bcd91684e6204f3ee3112516746007f9d1f6f Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 29 Aug 2024 12:58:02 +0200
Subject: [PATCH 46/85] Improve map interpolators and make them available in
 Python

---
 docs/pages/intro.rst                          |   2 +-
 docs/pages/parameters/index.rst               |   2 +-
 docs/pages/tutorials/cpp.rst                  |   6 +-
 docs/pages/tutorials/python.rst               |  12 +-
 docs/pages/tutorials/ros1.rst                 |   2 +-
 docs/python_api/index.rst                     |   3 -
 .../queries/nearest_neighbor_interpolation.cc |   9 +-
 .../python/queries/accelerated_queries.py     |   2 +
 examples/python/queries/multi_resolution.py   |  12 +-
 .../queries/nearest_neighbor_interpolation.py |  10 +-
 .../python/queries/trilinear_interpolation.py |  11 +-
 .../utils/query/impl/map_interpolator_inl.h   |  68 +++++
 .../core/utils/query/map_interpolator.h       |  56 +---
 .../core/utils/query/query_accelerator.h      |  10 +-
 library/python/src/maps.cc                    | 260 ++++++++++++------
 15 files changed, 304 insertions(+), 161 deletions(-)
 create mode 100644 library/cpp/include/wavemap/core/utils/query/impl/map_interpolator_inl.h

diff --git a/docs/pages/intro.rst b/docs/pages/intro.rst
index ef9d96b65..d44030aeb 100644
--- a/docs/pages/intro.rst
+++ b/docs/pages/intro.rst
@@ -47,7 +47,7 @@ For other citation styles, you can use the `Crosscite's citation formatter <http
 
 .. note::
 
-    Note that the code has significantly improved since the paper was written. Wavemap is now up to 10x faster, thanks to new multi-threaded measurement integrators, and uses up to 50% less RAM, by virtue of new memory efficient data structures inspired by `OpenVDB <https://github.com/AcademySoftwareFoundation/openvdb>`__.
+    The code has significantly improved since the paper was written. Wavemap is now up to 10x faster, thanks to new multi-threaded measurement integrators, and uses up to 50% less RAM, by virtue of new memory efficient data structures inspired by `OpenVDB <https://github.com/AcademySoftwareFoundation/openvdb>`__.
 
 .. only:: html
 
diff --git a/docs/pages/parameters/index.rst b/docs/pages/parameters/index.rst
index 5da431112..313caf312 100644
--- a/docs/pages/parameters/index.rst
+++ b/docs/pages/parameters/index.rst
@@ -20,7 +20,7 @@ You might have noticed two peculiarities in wavemap's configs.
 The first is that subsections often contain a **type** tag. This tag determines the type of the object that should be created for the corresponding element. For example, setting ``map/type: hashed_chunked_wavelet_octree`` tells wavemap to create a *HashedChunkedWaveletOctree* data structure to store the map.
 The second is that **units** are specified explicitly. This eliminates misunderstandings and enables automatic conversions from derived units, such as ``{ millimeters: 27.67 }`` or ``{ degrees: 180.0 }``, to SI base units, such as ``{ meters: 0.02767 }`` or ``{ radians: 3.14159 }``. Wavemap's code internally always operates in SI base units.
 
-.. note::
+.. tip::
 
     Wavemap's ROS server prints warnings for all unrecognized params at startup. This can be helpful for debugging and to quickly find typos in config param names.
 
diff --git a/docs/pages/tutorials/cpp.rst b/docs/pages/tutorials/cpp.rst
index a2e33a073..69827030c 100644
--- a/docs/pages/tutorials/cpp.rst
+++ b/docs/pages/tutorials/cpp.rst
@@ -4,7 +4,7 @@ C++ API
 
 In this tutorial, we illustrate how you can use wavemap's C++ API in your own projects.
 
-.. note::
+.. tip::
 
     An example package that combines the setup steps and code examples that follow can be found :gh_file:`here <examples/cpp>`.
 
@@ -106,9 +106,9 @@ Real coordinates
 ----------------
 Many applications require occupancy estimates at arbitrary 3D points, with real-valued coordinates. Such estimates are computed by interpolating the map.
 
-.. note::
+.. caution::
 
-    In case the query points are expressed in a different coordinate frame than the map, do not forget to transform them into the map frame before you continue.
+    If your query points are expressed in a different coordinate frame than the map, do not forget to transform them into the map frame before you continue.
 
 Nearest neighbor interpolation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
diff --git a/docs/pages/tutorials/python.rst b/docs/pages/tutorials/python.rst
index 44bc91a8d..75b340ccb 100644
--- a/docs/pages/tutorials/python.rst
+++ b/docs/pages/tutorials/python.rst
@@ -22,7 +22,7 @@ Code examples
 
 In the following sections, we provide sample code for common tasks. If you'd like to request examples for additional tasks or contribute new examples, please don't hesitate to `contact us <https://github.com/ethz-asl/wavemap/issues>`_.
 
-.. note::
+.. tip::
 
     All of the examples scripts that follow can be found :gh_file:`here <examples/python>`.
 
@@ -99,18 +99,22 @@ It is also possible to query lower resolution nodes, whose values correspond to
 
 Accelerators
 ^^^^^^^^^^^^
-In case you intend to look up multiple node values, we recommend using wavemap's query accelerator which traverses the octree significantly faster by caching parent nodes.
+If you need to look up multiple node values, we recommend using our batched query functions. These functions deliver significant speedups by utilizing wavemap's QueryAccelerator.
 
 .. literalinclude:: ../../../examples/python/queries/accelerated_queries.py
     :language: python
 
+.. note::
+
+    So far batched queries are only implemented for HashedWaveletOctree maps. We will add support for HashedChunkedWaveletOctree maps in the near future.
+
 Real coordinates
 ----------------
 Many applications require occupancy estimates at arbitrary 3D points, with real-valued coordinates. Such estimates are computed by interpolating the map.
 
-.. note::
+.. caution::
 
-    In case the query points are expressed in a different coordinate frame than the map, do not forget to transform them into the map frame before you continue.
+    If your query points are expressed in a different coordinate frame than the map, do not forget to transform them into the map frame before you continue.
 
 Nearest neighbor interpolation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
diff --git a/docs/pages/tutorials/ros1.rst b/docs/pages/tutorials/ros1.rst
index e03e76062..31ce6e936 100644
--- a/docs/pages/tutorials/ros1.rst
+++ b/docs/pages/tutorials/ros1.rst
@@ -44,7 +44,7 @@ Your own code
 *************
 We now briefly discuss how to set up your own ROS1 package to use wavemap, before proceeding to code examples.
 
-.. note::
+.. tip::
 
     An example package that combines the setup steps and code examples that follow can be found :gh_file:`here <examples/ros1>`.
 
diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst
index 6dd72f95b..c77d011c8 100644
--- a/docs/python_api/index.rst
+++ b/docs/python_api/index.rst
@@ -15,9 +15,6 @@ Python API
     :show-inheritance:
     :members:
 
-.. autoclass:: pywavemap.HashedWaveletOctreeQueryAccelerator
-    :members:
-
 .. autoclass:: pywavemap.OctreeIndex
     :members:
 
diff --git a/examples/cpp/queries/nearest_neighbor_interpolation.cc b/examples/cpp/queries/nearest_neighbor_interpolation.cc
index b1df52609..181316d41 100644
--- a/examples/cpp/queries/nearest_neighbor_interpolation.cc
+++ b/examples/cpp/queries/nearest_neighbor_interpolation.cc
@@ -13,13 +13,8 @@ int main(int, char**) {
   // Declare the point to query [in map frame]
   const Point3D query_point = Point3D::Zero();
 
-  // Compute the index that's nearest to the query point
-  const FloatingPoint min_cell_width_inv = 1.f / map->getMinCellWidth();
-  const Index3D nearest_neighbor_index =
-      convert::pointToNearestIndex(query_point, min_cell_width_inv);
-
-  // Query the map
+  // Query the value of the nearest cell in the map
   const FloatingPoint occupancy_log_odds =
-      map->getCellValue(nearest_neighbor_index);
+      interpolate::nearestNeighbor(*map, query_point);
   examples::doSomething(occupancy_log_odds);
 }
diff --git a/examples/python/queries/accelerated_queries.py b/examples/python/queries/accelerated_queries.py
index c8ea87b64..5520e422a 100644
--- a/examples/python/queries/accelerated_queries.py
+++ b/examples/python/queries/accelerated_queries.py
@@ -8,8 +8,10 @@
 # Vectorized query for a list of fixed-resolution indices
 indices = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
 values = your_map.getCellValues(indices)
+print(values)
 
 # Vectorized query for a list of node indices
 node_heights = np.random.randint(0, 6, size=(64 * 64 * 32, 1))
 node_indices = np.concatenate((node_heights, indices), axis=1)
 node_values = your_map.getCellValues(node_indices)
+print(node_values)
diff --git a/examples/python/queries/multi_resolution.py b/examples/python/queries/multi_resolution.py
index 750d70f64..20b617b7e 100644
--- a/examples/python/queries/multi_resolution.py
+++ b/examples/python/queries/multi_resolution.py
@@ -5,17 +5,17 @@
 # Load a map
 your_map = _dummy_objects.example_map()
 
-# Define the center point and the minimum width of the octree cell to query
+# Define the center point and the minimum width of your region of interest
 query_point = np.array([0.4, 0.5, 0.6])
 query_min_cell_width = 0.5  # in meters
 
-# Convert it to an octree node index
-map_min_cell_width = your_map.min_cell_width
+# Compute the index of the smallest node that covers it completely
 query_height = wave.convert.cell_width_to_height(query_min_cell_width,
-                                                 map_min_cell_width)
-query_index = wave.convert.point_to_node_index(query_point, map_min_cell_width,
+                                                 your_map.min_cell_width)
+query_index = wave.convert.point_to_node_index(query_point,
+                                               your_map.min_cell_width,
                                                query_height)
 
-# Query the map
+# Query the node's average occupancy
 occupancy_log_odds = your_map.getCellValue(query_index)
 print(occupancy_log_odds)
diff --git a/examples/python/queries/nearest_neighbor_interpolation.py b/examples/python/queries/nearest_neighbor_interpolation.py
index 5416498d9..6bda45d0f 100644
--- a/examples/python/queries/nearest_neighbor_interpolation.py
+++ b/examples/python/queries/nearest_neighbor_interpolation.py
@@ -7,5 +7,11 @@
 # Declare the point to query [in map frame]
 query_point = np.array([0.4, .5, 0.6])
 
-# TODO(victorr): Extend the bindings
-# TODO(victorr): Include vectorized version
+# Query a single point
+occupancy_log_odds = your_map.interpolateNearest(query_point)
+print(occupancy_log_odds)
+
+# Vectorized query for a list of points
+points = np.random.random(size=(64 * 64 * 32, 3))
+points_log_odds = your_map.interpolateNearest(points)
+print(points_log_odds)
diff --git a/examples/python/queries/trilinear_interpolation.py b/examples/python/queries/trilinear_interpolation.py
index 30d06fc8f..6b4d79ed8 100644
--- a/examples/python/queries/trilinear_interpolation.py
+++ b/examples/python/queries/trilinear_interpolation.py
@@ -7,6 +7,11 @@
 # Declare the point to query [in map frame]
 query_point = np.array([0.4, .5, 0.6])
 
-# TODO(victorr): Extend the bindings
-# TODO(victorr): Include vectorized version
-occupancy_log_odds = 0.0  # placeholder
+# Query a single point
+occupancy_log_odds = your_map.interpolateTrilinear(query_point)
+print(occupancy_log_odds)
+
+# Vectorized query for a list of points
+points = np.random.random(size=(64 * 64 * 32, 3))
+points_log_odds = your_map.interpolateTrilinear(points)
+print(points_log_odds)
diff --git a/library/cpp/include/wavemap/core/utils/query/impl/map_interpolator_inl.h b/library/cpp/include/wavemap/core/utils/query/impl/map_interpolator_inl.h
new file mode 100644
index 000000000..0d0058946
--- /dev/null
+++ b/library/cpp/include/wavemap/core/utils/query/impl/map_interpolator_inl.h
@@ -0,0 +1,68 @@
+#ifndef WAVEMAP_CORE_UTILS_QUERY_IMPL_MAP_INTERPOLATOR_INL_H_
+#define WAVEMAP_CORE_UTILS_QUERY_IMPL_MAP_INTERPOLATOR_INL_H_
+
+#include "wavemap/core/indexing/index_conversions.h"
+#include "wavemap/core/utils/data/eigen_checks.h"
+
+namespace wavemap::interpolate {
+template <typename MapT>
+FloatingPoint nearestNeighbor(MapT& map, const Point3D& position) {
+  // Compute the index of the cell closest to the query point
+  const FloatingPoint cell_width_inv = 1.f / map.getMinCellWidth();
+  const auto nearest_index =
+      wavemap::convert::pointToNearestIndex(position, cell_width_inv);
+  // Lookup and return its value
+  return map.getCellValue(nearest_index);
+}
+
+template <typename MapT>
+FloatingPoint trilinear(MapT& map, const Point3D& position) {
+  // Compute the index of the interpolation neighborhood's minimum corner
+  const FloatingPoint cell_width = map.getMinCellWidth();
+  const FloatingPoint cell_width_inv = 1.f / cell_width;
+  const auto min_corner_index =
+      wavemap::convert::pointToFloorIndex(position, cell_width_inv);
+
+  // Gather the values of the 8 neighbors
+  std::array<FloatingPoint, 8> cube_corners{};
+  for (int neighbor_idx = 0; neighbor_idx < 8; ++neighbor_idx) {
+    const Index3D offset{(neighbor_idx >> 2) & 1, (neighbor_idx >> 1) & 1,
+                         neighbor_idx & 1};
+    cube_corners[neighbor_idx] = map.getCellValue(min_corner_index + offset);
+  }
+
+  // Compute the offset between the query point, the min corner and max corner
+  const Point3D position_min_corner =
+      wavemap::convert::indexToCenterPoint(min_corner_index, cell_width);
+  // Offset to min corner
+  const Vector3D a = (position - position_min_corner) * cell_width_inv;
+  DCHECK_EIGEN_GE(a, Vector3D::Constant(0.f - kEpsilon));
+  DCHECK_EIGEN_LE(a, Vector3D::Constant(1.f + kEpsilon));
+  // Offset to max corner
+  const Vector3D a_comp = 1.f - a.array();
+  DCHECK_EIGEN_GE(a_comp, Vector3D::Constant(0.f - kEpsilon));
+  DCHECK_EIGEN_LE(a_comp, Vector3D::Constant(1.f + kEpsilon));
+
+  // Interpolate out the first dimension,
+  // reducing the cube into a square that contains the query point
+  std::array<FloatingPoint, 4> plane_corners{};
+  for (int corner_idx = 0; corner_idx < 4; ++corner_idx) {
+    plane_corners[corner_idx] = a_comp[0] * cube_corners[corner_idx] +
+                                a[0] * cube_corners[corner_idx + 0b100];
+  }
+
+  // Interpolate out the second dimension,
+  // reducing the square into a line segment that contains the query point
+  std::array<FloatingPoint, 2> line_corners{};
+  for (int side_idx = 0; side_idx < 2; ++side_idx) {
+    line_corners[side_idx] = a_comp[1] * plane_corners[side_idx] +
+                             a[1] * plane_corners[side_idx + 0b10];
+  }
+
+  // Interpolate out the third dimension,
+  // reducing the line segment into a single value
+  return a_comp[2] * line_corners[0] + a[2] * line_corners[1];
+}
+}  // namespace wavemap::interpolate
+
+#endif  // WAVEMAP_CORE_UTILS_QUERY_IMPL_MAP_INTERPOLATOR_INL_H_
diff --git a/library/cpp/include/wavemap/core/utils/query/map_interpolator.h b/library/cpp/include/wavemap/core/utils/query/map_interpolator.h
index 0b57860ae..0fff3596e 100644
--- a/library/cpp/include/wavemap/core/utils/query/map_interpolator.h
+++ b/library/cpp/include/wavemap/core/utils/query/map_interpolator.h
@@ -2,58 +2,16 @@
 #define WAVEMAP_CORE_UTILS_QUERY_MAP_INTERPOLATOR_H_
 
 #include "wavemap/core/common.h"
-#include "wavemap/core/indexing/index_conversions.h"
-#include "wavemap/core/map/map_base.h"
-#include "wavemap/core/utils/data/eigen_checks.h"
+#include "wavemap/core/utils/query/query_accelerator.h"
 
 namespace wavemap::interpolate {
-FloatingPoint trilinear(const wavemap::MapBase& map,
-                        const wavemap::Point3D& position) {
-  const FloatingPoint cell_width = map.getMinCellWidth();
-  const FloatingPoint cell_width_inv = 1.f / cell_width;
-  const auto min_corner_index =
-      wavemap::convert::pointToFloorIndex(position, cell_width_inv);
+template <typename MapT>
+FloatingPoint nearestNeighbor(MapT& map, const wavemap::Point3D& position);
 
-  // Gather the values of the 8 neighbors
-  std::array<FloatingPoint, 8> cube_corners{};
-  for (int neighbor_idx = 0; neighbor_idx < 8; ++neighbor_idx) {
-    const Index3D offset{(neighbor_idx >> 2) & 1, (neighbor_idx >> 1) & 1,
-                         neighbor_idx & 1};
-    cube_corners[neighbor_idx] = map.getCellValue(min_corner_index + offset);
-  }
-
-  // Compute the offset between the query point, the min corner and max corner
-  const Point3D position_min_corner =
-      wavemap::convert::indexToCenterPoint(min_corner_index, cell_width);
-  // Offset to min corner
-  const Vector3D a = (position - position_min_corner) * cell_width_inv;
-  DCHECK_EIGEN_GE(a, Vector3D::Constant(0.f - kEpsilon));
-  DCHECK_EIGEN_LE(a, Vector3D::Constant(1.f + kEpsilon));
-  // Offset to max corner
-  const Vector3D a_comp = 1.f - a.array();
-  DCHECK_EIGEN_GE(a_comp, Vector3D::Constant(0.f - kEpsilon));
-  DCHECK_EIGEN_LE(a_comp, Vector3D::Constant(1.f + kEpsilon));
-
-  // Interpolate out the first dimension,
-  // reducing the cube into a square that contains the query point
-  std::array<FloatingPoint, 4> plane_corners{};
-  for (int corner_idx = 0; corner_idx < 4; ++corner_idx) {
-    plane_corners[corner_idx] = a_comp[0] * cube_corners[corner_idx] +
-                                a[0] * cube_corners[corner_idx + 0b100];
-  }
-
-  // Interpolate out the second dimension,
-  // reducing the square into a line segment that contains the query point
-  std::array<FloatingPoint, 2> line_corners{};
-  for (int side_idx = 0; side_idx < 2; ++side_idx) {
-    line_corners[side_idx] = a_comp[1] * plane_corners[side_idx] +
-                             a[1] * plane_corners[side_idx + 0b10];
-  }
-
-  // Interpolate out the third dimension,
-  // reducing the line segment into a single value
-  return a_comp[2] * line_corners[0] + a[2] * line_corners[1];
-}
+template <typename MapT>
+FloatingPoint trilinear(MapT& map, const wavemap::Point3D& position);
 }  // namespace wavemap::interpolate
 
+#include "wavemap/core/utils/query/impl/map_interpolator_inl.h"
+
 #endif  // WAVEMAP_CORE_UTILS_QUERY_MAP_INTERPOLATOR_H_
diff --git a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
index 44e28139c..0e6c76569 100644
--- a/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
+++ b/library/cpp/include/wavemap/core/utils/query/query_accelerator.h
@@ -107,9 +107,14 @@ class QueryAccelerator<NdtreeBlockHash<CellDataT, dim>> {
 };
 
 /**
- * A class that accelerates queries by caching block and parent node  addresses
+ * A class that accelerates queries by caching block and parent node addresses
  * to speed up data structure traversals, and intermediate wavelet decompression
  * results to reduce redundant computation.
+ * @note This class is safe to use in a multi-threaded environment. However,
+ *       concurrent calls to a single instance from multiple threads are not.
+ *       Since the accelerator is lightweight and cheap to construct, we
+ *       recommend using a separate instance per thread for the best performance
+ *       and simplicity.
  */
 template <>
 class QueryAccelerator<HashedWaveletOctree> {
@@ -133,6 +138,9 @@ class QueryAccelerator<HashedWaveletOctree> {
   //! Query the value of the map at a given octree node index
   FloatingPoint getCellValue(const OctreeIndex& index);
 
+  //! Convenience function to get the map's minimum cell width
+  FloatingPoint getMinCellWidth() const { return map_.getMinCellWidth(); }
+
  private:
   using BlockIndex = HashedWaveletOctree::BlockIndex;
   using NodeType = HashedWaveletOctree::Block::NodeType;
diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc
index a4a43152a..dfb003e37 100644
--- a/library/python/src/maps.cc
+++ b/library/python/src/maps.cc
@@ -7,6 +7,7 @@
 #include <wavemap/core/map/hashed_wavelet_octree.h>
 #include <wavemap/core/map/map_base.h>
 #include <wavemap/core/map/map_factory.h>
+#include <wavemap/core/utils/query/map_interpolator.h>
 #include <wavemap/core/utils/query/query_accelerator.h>
 #include <wavemap/io/file_conversions.h>
 
@@ -14,81 +15,90 @@ using namespace nb::literals;  // NOLINT
 
 namespace wavemap {
 void add_map_bindings(nb::module_& m) {
-  auto map_base =
-      nb::class_<MapBase>(m, "Map", "Base class for wavemap maps.")
-          .def_prop_ro("empty", &MapBase::empty, "Whether the map is empty.")
-          .def_prop_ro("size", &MapBase::size,
-                       "The number of cells or nodes in the map, for fixed or "
-                       "multi-resolution maps, respectively.")
-          .def("threshold", &MapBase::threshold,
-               "Threshold the occupancy values of all cells in the map to stay "
-               "within the range specified by its min_log_odds and "
-               "max_log_odds.")
-          .def(
-              "prune", &MapBase::prune,
-              "Free up memory by pruning nodes that are no longer needed. Note "
-              "that this pruning operation is lossless and does not alter the "
-              "estimated occupancy posterior.")
-          .def("pruneSmart", &MapBase::pruneSmart,
-               "Similar to prune(), but avoids de-allocating nodes that were "
-               "recently updated and will likely be used again in the near "
-               "future.")
-          .def("clear", &MapBase::clear, "Erase all cells in the map.")
-          .def_prop_ro(
-              "min_cell_width", &MapBase::getMinCellWidth,
-              "Maximum map resolution, set as width of smallest cell it "
-              "can represent.")
-          .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds,
-                       "Lower threshold for the occupancy values stored in the "
-                       "map, in log-odds.")
-          .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds,
-                       "Upper threshold for the occupancy values stored in the "
-                       "map, in log-odds.")
-          .def_prop_ro("memory_usage", &MapBase::getMemoryUsage,
-                       "The amount of memory used by the map, in bytes.")
-          .def_prop_ro(
-              "tree_height", &MapBase::getTreeHeight,
-              "Height of the octree used to store the map. Note that this "
-              "value is only defined for multi-resolution maps.")
-          .def_prop_ro("min_index", &MapBase::getMinIndex,
-                       "Index of the minimum corner of the map's Axis Aligned "
-                       "Bounding Box.")
-          .def_prop_ro("max_index", &MapBase::getMaxIndex,
-                       "Index of the maximum corner of the map's Axis Aligned "
-                       "Bounding Box.")
-          .def("getCellValue", &MapBase::getCellValue, "index"_a,
-               "Query the value of the map at a given index.")
-          .def("setCellValue", &MapBase::setCellValue, "index"_a,
-               "new_value"_a
-               "Set the value of the map at a given index.")
-          .def("addToCellValue", &MapBase::addToCellValue, "index"_a,
-               "update"_a, "Increment the value of the map at a given index.")
-          .def_static(
-              "create",
-              [](const param::Value& params) -> std::shared_ptr<MapBase> {
-                return MapFactory::create(params);
-              },
-              nb::sig("def create(parameters: dict) -> Map"), "parameters"_a,
-              "Create a new map based on the given settings.")
-          .def_static(
-              "load",
-              [](const std::filesystem::path& file_path)
-                  -> std::shared_ptr<MapBase> {
-                std::shared_ptr<MapBase> map;
-                if (wavemap::io::fileToMap(file_path, map)) {
-                  return map;
-                }
-                return nullptr;
-              },
-              "file_path"_a, "Load a wavemap map from a .wvmp file.")
-          .def(
-              "store",
-              [](const MapBase& self, const std::filesystem::path& file_path)
-                  -> bool { return wavemap::io::mapToFile(self, file_path); },
-              "file_path"_a, "Store a wavemap map as a .wvmp file.");
+  nb::class_<MapBase>(m, "Map", "Base class for wavemap maps.")
+      .def_prop_ro("empty", &MapBase::empty, "Whether the map is empty.")
+      .def_prop_ro("size", &MapBase::size,
+                   "The number of cells or nodes in the map, for fixed or "
+                   "multi-resolution maps, respectively.")
+      .def("threshold", &MapBase::threshold,
+           "Threshold the occupancy values of all cells in the map to stay "
+           "within the range specified by its min_log_odds and max_log_odds.")
+      .def("prune", &MapBase::prune,
+           "Free up memory by pruning nodes that are no longer needed. Note "
+           "that this pruning operation is lossless and does not alter the "
+           "estimated occupancy posterior.")
+      .def("pruneSmart", &MapBase::pruneSmart,
+           "Similar to prune(), but avoids de-allocating nodes that were "
+           "recently updated and will likely be used again in the near future.")
+      .def("clear", &MapBase::clear, "Erase all cells in the map.")
+      .def_prop_ro("min_cell_width", &MapBase::getMinCellWidth,
+                   "Maximum map resolution, set as width of smallest cell it "
+                   "can represent.")
+      .def_prop_ro("min_log_odds", &MapBase::getMinLogOdds,
+                   "Lower threshold for the occupancy values stored in the "
+                   "map, in log-odds.")
+      .def_prop_ro("max_log_odds", &MapBase::getMaxLogOdds,
+                   "Upper threshold for the occupancy values stored in the "
+                   "map, in log-odds.")
+      .def_prop_ro("memory_usage", &MapBase::getMemoryUsage,
+                   "The amount of memory used by the map, in bytes.")
+      .def_prop_ro("tree_height", &MapBase::getTreeHeight,
+                   "Height of the octree used to store the map. Note that this "
+                   "value is only defined for multi-resolution maps.")
+      .def_prop_ro("min_index", &MapBase::getMinIndex,
+                   "Index of the minimum corner of the map's Axis Aligned "
+                   "Bounding Box.")
+      .def_prop_ro("max_index", &MapBase::getMaxIndex,
+                   "Index of the maximum corner of the map's Axis Aligned "
+                   "Bounding Box.")
+      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+           "Query the value of the map at a given index.")
+      .def("setCellValue", &MapBase::setCellValue, "index"_a,
+           "new_value"_a
+           "Set the value of the map at a given index.")
+      .def("addToCellValue", &MapBase::addToCellValue, "index"_a, "update"_a,
+           "Increment the value of the map at a given index.")
+      .def(
+          "interpolateNearest",
+          [](const MapBase& self, const Point3D& position) {
+            return interpolate::nearestNeighbor(self, position);
+          },
+          "position"_a,
+          "Query the map's value at a point using nearest neighbor "
+          "interpolation.")
+      .def(
+          "interpolateTrilinear",
+          [](const MapBase& self, const Point3D& position) {
+            return interpolate::trilinear(self, position);
+          },
+          "position"_a,
+          "Query the map's value at a point using trilinear interpolation.")
+      .def_static(
+          "create",
+          [](const param::Value& params) -> std::shared_ptr<MapBase> {
+            return MapFactory::create(params);
+          },
+          nb::sig("def create(parameters: dict) -> Map"), "parameters"_a,
+          "Create a new map based on the given settings.")
+      .def_static(
+          "load",
+          [](const std::filesystem::path& file_path)
+              -> std::shared_ptr<MapBase> {
+            std::shared_ptr<MapBase> map;
+            if (wavemap::io::fileToMap(file_path, map)) {
+              return map;
+            }
+            return nullptr;
+          },
+          "file_path"_a, "Load a wavemap map from a .wvmp file.")
+      .def(
+          "store",
+          [](const MapBase& self, const std::filesystem::path& file_path)
+              -> bool { return wavemap::io::mapToFile(self, file_path); },
+          "file_path"_a, "Store a wavemap map as a .wvmp file.");
 
-  nb::class_<HashedWaveletOctree>(
-      m, "HashedWaveletOctree", map_base,
+  nb::class_<HashedWaveletOctree, MapBase>(
+      m, "HashedWaveletOctree",
       "A class that stores maps using hashed wavelet octrees.")
       .def("getCellValue", &MapBase::getCellValue, "index"_a,
            "Query the value of the map at a given index.")
@@ -104,7 +114,7 @@ void add_map_bindings(nb::module_& m) {
                  indices) {
             // Create a query accelerator
             QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
-            // Create nb::ndarray view for efficient access
+            // Create nb::ndarray view for efficient access to the query indices
             const auto index_view = indices.view();
             const auto num_queries = index_view.shape(0);
             // Allocate and populate raw results array
@@ -133,7 +143,7 @@ void add_map_bindings(nb::module_& m) {
                  indices) {
             // Create a query accelerator
             QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
-            // Create nb::ndarray view for efficient access
+            // Create nb::ndarray view for efficient access to the query indices
             auto index_view = indices.view();
             const auto num_queries = index_view.shape(0);
             // Allocate and populate raw results array
@@ -156,10 +166,85 @@ void add_map_bindings(nb::module_& m) {
           },
           "node_index_list"_a,
           "Query the map at the given node indices, provided as a matrix with "
-          "one (height, x, y, z) node index per row.");
+          "one (height, x, y, z) node index per row.")
+      .def(
+          "interpolateNearest",
+          [](const MapBase& self, const Point3D& position) {
+            return interpolate::nearestNeighbor(self, position);
+          },
+          "position"_a,
+          "Query the map's value at a point using nearest neighbor "
+          "interpolation.")
+      .def(
+          "interpolateTrilinear",
+          [](const MapBase& self, const Point3D& position) {
+            return interpolate::trilinear(self, position);
+          },
+          "position"_a,
+          "Query the map's value at a point using trilinear interpolation.")
+      .def(
+          "interpolateNearest",
+          [](const HashedWaveletOctree& self,
+             const nb::ndarray<FloatingPoint, nb::shape<-1, 3>,
+                               nb::device::cpu>& positions) {
+            // Create a query accelerator
+            QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
+            // Create nb::ndarray view for efficient access to the query points
+            const auto positions_view = positions.view();
+            const auto num_queries = positions_view.shape(0);
+            // Allocate and populate raw results array
+            auto* results = new float[num_queries];
+            for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
+              results[query_idx] = interpolate::nearestNeighbor(
+                  query_accelerator,
+                  {positions_view(query_idx, 0), positions_view(query_idx, 1),
+                   positions_view(query_idx, 2)});
+            }
+            // Create Python capsule that deallocates the results array when
+            // all references to it expire
+            nb::capsule owner(results, [](void* p) noexcept {
+              delete[] reinterpret_cast<float*>(p);
+            });
+            // Return results as numpy array
+            return nb::ndarray<nb::numpy, float>{
+                results, {num_queries, 1u}, owner};
+          },
+          "position_list"_a,
+          "Query the map's value at the given points using nearest neighbor "
+          "interpolation.")
+      .def(
+          "interpolateTrilinear",
+          [](const HashedWaveletOctree& self,
+             const nb::ndarray<FloatingPoint, nb::shape<-1, 3>,
+                               nb::device::cpu>& positions) {
+            // Create a query accelerator
+            QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
+            // Create nb::ndarray view for efficient access to the query points
+            const auto positions_view = positions.view();
+            const auto num_queries = positions_view.shape(0);
+            // Allocate and populate raw results array
+            auto* results = new float[num_queries];
+            for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
+              results[query_idx] = interpolate::trilinear(
+                  query_accelerator,
+                  {positions_view(query_idx, 0), positions_view(query_idx, 1),
+                   positions_view(query_idx, 2)});
+            }
+            // Create Python capsule that deallocates the results array when
+            // all references to it expire
+            nb::capsule owner(results, [](void* p) noexcept {
+              delete[] reinterpret_cast<float*>(p);
+            });
+            // Return results as numpy array
+            return nb::ndarray<nb::numpy, float>{
+                results, {num_queries, 1u}, owner};
+          },
+          "position_list"_a,
+          "Query the map's value at the given points using trilinear "
+          "interpolation.");
 
-  nb::class_<HashedChunkedWaveletOctree>(
-      m, "HashedChunkedWaveletOctree", map_base,
+  nb::class_<HashedChunkedWaveletOctree, MapBase>(
+      m, "HashedChunkedWaveletOctree",
       "A class that stores maps using hashed chunked wavelet octrees.")
       .def("getCellValue", &MapBase::getCellValue, "index"_a,
            "Query the value of the map at a given index.")
@@ -167,6 +252,21 @@ void add_map_bindings(nb::module_& m) {
            nb::overload_cast<const OctreeIndex&>(
                &HashedChunkedWaveletOctree::getCellValue, nb::const_),
            "node_index"_a,
-           "Query the value of the map at a given octree node index.");
+           "Query the value of the map at a given octree node index.")
+      .def(
+          "interpolateNearest",
+          [](const MapBase& self, const Point3D& position) {
+            return interpolate::nearestNeighbor(self, position);
+          },
+          "position"_a,
+          "Query the map's value at a point using nearest neighbor "
+          "interpolation.")
+      .def(
+          "interpolateTrilinear",
+          [](const MapBase& self, const Point3D& position) {
+            return interpolate::trilinear(self, position);
+          },
+          "position"_a,
+          "Query the map's value at a point using trilinear interpolation.");
 }
 }  // namespace wavemap

From 4965480c7e83bf889e3b9d4788c6cf336da60a85 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Fri, 30 Aug 2024 12:52:49 +0200
Subject: [PATCH 47/85] Improve release note preparation script

---
 tooling/scripts/prepare_release.py | 29 +++++++++++++++++------------
 1 file changed, 17 insertions(+), 12 deletions(-)

diff --git a/tooling/scripts/prepare_release.py b/tooling/scripts/prepare_release.py
index 2cbc365a0..37bf8df1b 100755
--- a/tooling/scripts/prepare_release.py
+++ b/tooling/scripts/prepare_release.py
@@ -87,7 +87,7 @@ def draft_release_notes():
     out += "### Libraries\n"
     pkg = pkgs["libraries"][0]
     out += f"* [{pkg.type}](https://github.com/ethz-asl/wavemap/blob/"
-    out += "v{new_version_str}/{pkg.current_path}/CHANGELOG.rst)\n"
+    out += f"v{new_version_str}/{pkg.current_path}/CHANGELOG.rst)\n"
     out += "\n"
 
     out += "### Interfaces\n"
@@ -105,19 +105,24 @@ def draft_release_notes():
 
     out += "# Upgrade notes\n"
     out += "Upgrade instructions for\n"
-    out += "* Catkin\n"
-    out += "  * Go to your catkin workspace src directory: "
+    out += "* C++ Library\n"
+    out += "  * For instructions on setting up wavemap as a standalone CMake "
+    out += "project, please refer to [our docs]"
+    out += "(https://ethz-asl.github.io/wavemap/pages/installation/cmake)\n"
+    out += "* ROS1\n"
+    out += "  * Catkin\n"
+    out += "    * Go to your catkin workspace src directory: "
     out += "`cd ~/catkin_ws/src`\n"
-    out += "  * Pull the newest wavemap code:"
+    out += "    * Pull the newest wavemap code:"
     out += "`cd wavemap && git checkout main && git pull`\n"
-    out += "  * Rebuild wavemap: `catkin build wavemap_all`\n"
-    out += "* Docker\n"
-    out += "  * `docker build --tag=wavemap_ros1 "
-    out += f"--build-arg=\"VERSION=v{new_version_str}\" -"
-    out += "<<< $(curl -s https://raw.githubusercontent.com/ethz-asl/wavemap/"
-    out += "main/tooling/docker/ros1/incremental.Dockerfile)`\n"
-    out += "For more info, see the [installation page](https://"
-    out += "ethz-asl.github.io/wavemap/pages/installation) in the docs.)"
+    out += "    * Rebuild wavemap: `catkin build wavemap_all`\n"
+    out += "  * Docker\n"
+    out += "    * `docker build --tag=wavemap_ros1 "
+    out += f"--build-arg=\"VERSION=v{new_version_str}\" -<<< $(curl -s https://"
+    out += f"raw.githubusercontent.com/ethz-asl/wavemap/v{new_version_str}"
+    out += "/tooling/docker/ros1/incremental.Dockerfile)`\n\n"
+    out += "For more info, see our guides on [installing wavemap](https://"
+    out += "ethz-asl.github.io/wavemap/pages/installation)."
     out += "\n"
 
     print(out)

From cff116033c01c2774f15823bbcc44a0106152fdf Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Sat, 31 Aug 2024 19:37:26 +0200
Subject: [PATCH 48/85] Improve consistency and reduce Python API install steps

---
 docs/pages/installation/cmake.rst       |  8 ++--
 docs/pages/installation/python.rst      | 59 ++++++++++++++-----------
 examples/cpp/CMakeLists.txt             |  7 +--
 library/python/CMakeLists.txt           |  7 +--
 tooling/docker/cpp/alpine.Dockerfile    |  7 +--
 tooling/docker/cpp/debian.Dockerfile    |  7 +--
 tooling/docker/python/alpine.Dockerfile |  7 +--
 tooling/docker/python/debian.Dockerfile |  7 +--
 8 files changed, 62 insertions(+), 47 deletions(-)

diff --git a/docs/pages/installation/cmake.rst b/docs/pages/installation/cmake.rst
index e854f45ee..8a6119c05 100644
--- a/docs/pages/installation/cmake.rst
+++ b/docs/pages/installation/cmake.rst
@@ -24,14 +24,14 @@ The fastest way to include wavemap in an existing CMake project is to use FetchC
 
 .. code-block:: cmake
 
-  set(WAVEMAP_TAG develop/v2.0)
+  set(WAVEMAP_VERSION main)  # Select a git branch, tag or commit
 
   cmake_minimum_required(VERSION 3.18)
-  message(STATUS "Fetching wavemap ${WAVEMAP_TAG} from GitHub")
+  message(STATUS "Loading wavemap from GitHub (ref ${WAVEMAP_VERSION})")
   include(FetchContent)
   FetchContent_Declare(wavemap
       GIT_REPOSITORY https://github.com/ethz-asl/wavemap.git
-      GIT_TAG ${WAVEMAP_TAG}
+      GIT_TAG ${WAVEMAP_VERSION}
       GIT_SHALLOW 1
       SOURCE_SUBDIR library/cpp)
   FetchContent_MakeAvailable(wavemap)
@@ -87,7 +87,7 @@ To build wavemap's C++ Docker image, simply run:
 
       docker build --tag=wavemap_cpp --pull - <<< $(curl -s https://raw.githubusercontent.com/ethz-asl/wavemap/main/tooling/docker/cpp/debian.Dockerfile)
 
-This will create a local image on your machine containing the latest version of wavemap's C++ library. You can give the local image a different name by modifying the ``--tag=wavemap_cpp`` argument. By default, the image will be built using the latest code on wavemap's ``main`` branch. To specify a specific release or branch, such as `develop/v2.0`, add the ``--build-arg="WAVEMAP_TAG=develop/v2.0"`` argument.
+This will create a local image on your machine containing the latest version of wavemap's C++ library. You can give the local image a different name by modifying the ``--tag=wavemap_cpp`` argument. By default, the image will be built using the latest code on wavemap's ``main`` branch. To specify a specific branch, commit or release, such as `v2.1.0`, add the ``--build-arg="WAVEMAP_VERSION=v2.1.0"`` argument.
 
 Native install
 **************
diff --git a/docs/pages/installation/python.rst b/docs/pages/installation/python.rst
index a7a58ab67..8ae3ab3d4 100644
--- a/docs/pages/installation/python.rst
+++ b/docs/pages/installation/python.rst
@@ -3,9 +3,9 @@ Python (pip)
 .. highlight:: bash
 .. rstcheck: ignore-directives=tab-set-code
 
-We will make pywavemap available through PyPI soon. In the meantime, you can install it directly from source using pip.
+We're still working on making pywavemap available through PyPI. In the meantime, you can build and install it locally with pip, which takes less than two minutes and optimizes the build for your specific computer.
 
-If you only plan to use pywavemap without modifying its code, a regular install is easiest. However, if you are actively working on wavemap's C++ or Python libraries, we recommend using the editable installation method for fast, incremental rebuilds.
+If you plan to use pywavemap without changing its code, a regular installation is easiest. However, if you're modifying wavemap's C++ or Python libraries, we recommend using the editable installation method for fast, incremental rebuilds.
 
 Regular install
 ***************
@@ -29,24 +29,6 @@ First, make sure the necessary dependencies to build C++ and Python packages are
       apk add git build-base python3-dev py3-pip
       apk add python3-venv  # If you use virtual environments
 
-.. _python-install-clone-repo:
-
-Next, clone wavemap's code to your machine:
-
-.. tab-set-code::
-
-    .. code-block:: SSH
-      :class: no-header
-
-      cd ~/
-      git clone git@github.com:ethz-asl/wavemap.git
-
-    .. code-block:: HTTPS
-      :class: no-header
-
-      cd ~/
-      git clone https://github.com/ethz-asl/wavemap.git
-
 .. _python-install-setup-venv:
 
 *Optional:* We recommend using a virtual environment to isolate your Python dependencies. Create and activate it with the following commands:
@@ -67,16 +49,43 @@ Next, clone wavemap's code to your machine:
       python3 -m venv <path_to_new_virtual_env>
       source <path_to_new_virtual_env>/bin/activate
 
-You can then install pywavemap by running::
+You can then build and install the latest version or a specific version of pywavemap by running:
+
+.. tab-set-code::
+
+    .. code-block:: Latest
+      :class: no-header
+
+      pip3 install git+https://github.com/ethz-asl/wavemap#subdirectory=library/python
 
-    cd ~/wavemap/library/python
-    pip3 install .
+    .. code-block:: Specific
+      :class: no-header
+
+      # Select a specific git branch, tag or commit using @...
+      # For example, to install version v2.1.0, run
+      pip3 install git+https://github.com/ethz-asl/wavemap@v2.1.0#subdirectory=library/python
 
 Editable install
 ****************
-If you're interested in modifying wavemap's code, you can save time by enabling incremental builds.
+If you're interested in modifying wavemap's code, you can save time by enabling incremental rebuilds.
+
+The general steps are similar to those for a regular installation. Ensure your machine is :ref:`ready to build C++ and Python packages <python-install-build-deps>` and, optionally, :ref:`set up a virtual environment <python-install-setup-venv>`.
+
+Next, clone wavemap's code to your machine:
+
+.. tab-set-code::
+
+    .. code-block:: SSH
+      :class: no-header
+
+      cd ~/
+      git clone git@github.com:ethz-asl/wavemap.git
 
-The general steps are similar to those for a regular installation. Ensure your machine is :ref:`ready to build C++ and Python packages <python-install-build-deps>` and that you've :ref:`cloned the code <python-install-clone-repo>`. Optionally, you can :ref:`set up a virtual environment <python-install-setup-venv>`.
+    .. code-block:: HTTPS
+      :class: no-header
+
+      cd ~/
+      git clone https://github.com/ethz-asl/wavemap.git
 
 Since editable installs are no longer built in an isolated environment, all build dependencies must be available on your system::
 
diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt
index f2a3a29bd..43fd5f7a0 100644
--- a/examples/cpp/CMakeLists.txt
+++ b/examples/cpp/CMakeLists.txt
@@ -15,14 +15,15 @@ else ()
   if (wavemap_FOUND)
     message(STATUS "Loading wavemap library installed on system")
   else () # Otherwise, fetch wavemap's code from GitHub
-    set(WAVEMAP_TAG develop/v2.0)
-    message(STATUS "Loading wavemap library from GitHub (tag ${WAVEMAP_TAG})")
+    set(WAVEMAP_VERSION main)  # Select a git branch, tag or commit
+    message(STATUS
+        "Loading wavemap library from GitHub (ref ${WAVEMAP_VERSION})")
     cmake_minimum_required(VERSION 3.18)
     include(FetchContent)
     FetchContent_Declare(
         ext_wavemap PREFIX wavemap
         GIT_REPOSITORY https://github.com/ethz-asl/wavemap.git
-        GIT_TAG ${WAVEMAP_TAG}
+        GIT_TAG ${WAVEMAP_VERSION}
         GIT_SHALLOW 1
         SOURCE_SUBDIR library/cpp)
     FetchContent_MakeAvailable(ext_wavemap)
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 7b8f62330..17d8d74fa 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -32,14 +32,15 @@ if (NOT TARGET wavemap::wavemap_core)
     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library/cpp
         ${CMAKE_CURRENT_BINARY_DIR}/wavemap)
   else ()
-    set(WAVEMAP_TAG feature/pywavemap)
-    message(STATUS "Loading wavemap library from GitHub (tag ${WAVEMAP_TAG})")
+    set(WAVEMAP_VERSION main)
+    message(STATUS
+        "Loading wavemap library from GitHub (ref ${WAVEMAP_VERSION})")
     cmake_minimum_required(VERSION 3.18)
     include(FetchContent)
     FetchContent_Declare(
         ext_wavemap PREFIX wavemap
         GIT_REPOSITORY https://github.com/ethz-asl/wavemap.git
-        GIT_TAG ${WAVEMAP_TAG}
+        GIT_TAG ${WAVEMAP_VERSION}
         GIT_SHALLOW 1
         SOURCE_SUBDIR library/cpp)
     FetchContent_MakeAvailable(ext_wavemap)
diff --git a/tooling/docker/cpp/alpine.Dockerfile b/tooling/docker/cpp/alpine.Dockerfile
index ffd56e6e1..cc7f4c0ea 100644
--- a/tooling/docker/cpp/alpine.Dockerfile
+++ b/tooling/docker/cpp/alpine.Dockerfile
@@ -1,14 +1,15 @@
-ARG WAVEMAP_TAG=main
+# Select a git branch, tag or commit
+ARG WAVEMAP_VERSION=main
 
 FROM alpine:3.20
 
-ARG WAVEMAP_TAG
+ARG WAVEMAP_VERSION
 
 # hadolint ignore=DL3018
 RUN apk add --no-cache cmake build-base git eigen-dev glog-dev boost-dev
 
 # hadolint ignore=DL3059
-RUN git clone --branch ${WAVEMAP_TAG} https://github.com/ethz-asl/wavemap.git
+RUN git clone --branch ${WAVEMAP_VERSION} https://github.com/ethz-asl/wavemap.git
 
 WORKDIR /wavemap/library/cpp
 RUN cmake -S . -B build && \
diff --git a/tooling/docker/cpp/debian.Dockerfile b/tooling/docker/cpp/debian.Dockerfile
index 501828d79..4e16608bc 100644
--- a/tooling/docker/cpp/debian.Dockerfile
+++ b/tooling/docker/cpp/debian.Dockerfile
@@ -1,8 +1,9 @@
-ARG WAVEMAP_TAG=main
+# Select a git branch, tag or commit
+ARG WAVEMAP_VERSION=main
 
 FROM debian:11.10
 
-ARG WAVEMAP_TAG
+ARG WAVEMAP_VERSION
 
 # hadolint ignore=DL3008
 RUN apt-get update && \
@@ -11,7 +12,7 @@ RUN apt-get update && \
     libeigen3-dev libgoogle-glog-dev libboost-dev && \
     rm -rf /var/lib/apt/lists/*
 
-RUN git clone --branch ${WAVEMAP_TAG} https://github.com/ethz-asl/wavemap.git
+RUN git clone --branch ${WAVEMAP_VERSION} https://github.com/ethz-asl/wavemap.git
 
 WORKDIR /wavemap/library/cpp
 RUN cmake -S . -B build && \
diff --git a/tooling/docker/python/alpine.Dockerfile b/tooling/docker/python/alpine.Dockerfile
index 82ed993ff..754458341 100644
--- a/tooling/docker/python/alpine.Dockerfile
+++ b/tooling/docker/python/alpine.Dockerfile
@@ -1,14 +1,15 @@
-ARG WAVEMAP_TAG=main
+# Select a git branch, tag or commit
+ARG WAVEMAP_VERSION=main
 
 FROM alpine:3.20
 
-ARG WAVEMAP_TAG
+ARG WAVEMAP_VERSION
 
 # hadolint ignore=DL3018
 RUN apk add --no-cache git build-base python3-dev py3-pip
 
 # hadolint ignore=DL3059
-RUN git clone --branch ${WAVEMAP_TAG} https://github.com/ethz-asl/wavemap.git
+RUN git clone --branch ${WAVEMAP_VERSION} https://github.com/ethz-asl/wavemap.git
 
 WORKDIR /wavemap/library/python
 # hadolint ignore=DL3042
diff --git a/tooling/docker/python/debian.Dockerfile b/tooling/docker/python/debian.Dockerfile
index f421da4a2..e61a380fb 100644
--- a/tooling/docker/python/debian.Dockerfile
+++ b/tooling/docker/python/debian.Dockerfile
@@ -1,8 +1,9 @@
-ARG WAVEMAP_TAG=main
+# Select a git branch, tag or commit
+ARG WAVEMAP_VERSION=main
 
 FROM debian:11.10
 
-ARG WAVEMAP_TAG
+ARG WAVEMAP_VERSION
 
 # hadolint ignore=DL3008
 RUN apt-get update && \
@@ -10,7 +11,7 @@ RUN apt-get update && \
     git build-essential python3-dev python3-pip && \
     rm -rf /var/lib/apt/lists/*
 
-RUN git clone --branch ${WAVEMAP_TAG} https://github.com/ethz-asl/wavemap.git
+RUN git clone --branch ${WAVEMAP_VERSION} https://github.com/ethz-asl/wavemap.git
 
 WORKDIR /wavemap/library/python
 # hadolint ignore=DL3042

From 6efdbc296f4a1a4865e219292e354c57d32746f0 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 2 Sep 2024 13:51:02 +0200
Subject: [PATCH 49/85] Start adding tests for Python API

---
 examples/python/panoptic_mapping.py           |  2 +-
 .../panoptic_mapping_flat_data_player.py      |  2 +-
 library/python/pyproject.toml                 |  6 ++
 library/python/test/test_pywavemap.py         | 67 ++++++++++++++++
 tooling/git_hook_configs/.pylintrc            | 77 +------------------
 5 files changed, 78 insertions(+), 76 deletions(-)
 create mode 100644 library/python/test/test_pywavemap.py

diff --git a/examples/python/panoptic_mapping.py b/examples/python/panoptic_mapping.py
index 5c5c70d49..b1ab3d379 100644
--- a/examples/python/panoptic_mapping.py
+++ b/examples/python/panoptic_mapping.py
@@ -10,7 +10,7 @@
 
 
 class DataLoader():
-    # pylint: disable=R0902
+
     def __init__(self, params, data_path):
         self.data_path = data_path
 
diff --git a/interfaces/ros1/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py b/interfaces/ros1/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py
index 09013a86c..bb26eb97c 100755
--- a/interfaces/ros1/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py
+++ b/interfaces/ros1/wavemap_ros/scripts/panoptic_mapping_flat_data_player.py
@@ -21,7 +21,7 @@
 
 
 class FlatDataPlayer():
-    # pylint: disable=R0902
+    # pylint: disable=too-many-instance-attributes
     def __init__(self):
         """  Initialize ros node and read params """
         # params
diff --git a/library/python/pyproject.toml b/library/python/pyproject.toml
index 654d0c7ea..2139aaa15 100644
--- a/library/python/pyproject.toml
+++ b/library/python/pyproject.toml
@@ -16,6 +16,9 @@ classifiers = ["License :: BSD3"]
 [project.urls]
 Homepage = "https://github.com/ethz-asl/wavemap"
 
+[project.optional-dependencies]
+test = ["pytest"]
+
 [tool.scikit-build]
 build-dir = "build/{wheel_tag}"
 minimum-version = "0.4"
@@ -30,3 +33,6 @@ skip = ["cp38-*", "pp38-*"]
 [tool.cibuildwheel.macos]
 environment = "MACOSX_DEPLOYMENT_TARGET=10.14"
 archs = ["auto64", "arm64"]
+
+[tool.pytest.ini_options]
+testpaths = ['test']
diff --git a/library/python/test/test_pywavemap.py b/library/python/test/test_pywavemap.py
new file mode 100644
index 000000000..185cfdd5b
--- /dev/null
+++ b/library/python/test/test_pywavemap.py
@@ -0,0 +1,67 @@
+# pylint: disable=import-outside-toplevel
+def load_test_map():
+    import pywavemap
+    return pywavemap.Map.load(
+        "/home/victor/data/wavemaps/newer_college_mine_10cm.wvmp")
+
+
+def test_import():
+    import pywavemap
+
+    assert pywavemap is not None
+
+
+def test_batched_fixed_resolution_queries():
+    import numpy as np
+
+    test_map = load_test_map()
+
+    cell_indices = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
+    cell_values = test_map.getCellValues(cell_indices)
+    for cell_idx in range(cell_indices.shape[0]):
+        cell_index = cell_indices[cell_idx, :]
+        cell_value = test_map.getCellValue(cell_index)
+        assert cell_values[cell_idx] == cell_value
+
+
+def test_batched_multi_resolution_queries():
+    import numpy as np
+    import pywavemap
+
+    test_map = load_test_map()
+
+    cell_positions = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
+    cell_heights = np.random.randint(0, 6, size=(64 * 64 * 32, 1))
+    cell_indices = np.concatenate((cell_heights, cell_positions), axis=1)
+    cell_values = test_map.getCellValues(cell_indices)
+    for cell_idx in range(cell_positions.shape[0]):
+        cell_index = pywavemap.OctreeIndex(cell_heights[cell_idx],
+                                           cell_positions[cell_idx, :])
+        cell_value = test_map.getCellValue(cell_index)
+        assert cell_values[cell_idx] == cell_value
+
+
+def test_batched_nearest_neighbor_interpolation():
+    import numpy as np
+
+    test_map = load_test_map()
+
+    points = np.random.random(size=(64 * 64 * 32, 3))
+    points_log_odds = test_map.interpolateNearest(points)
+    for point_idx in range(points.shape[0]):
+        point = points[point_idx, :]
+        point_log_odds = test_map.interpolateNearest(point)
+        assert points_log_odds[point_idx] == point_log_odds
+
+
+def test_batched_trilinear_interpolation():
+    import numpy as np
+
+    test_map = load_test_map()
+
+    points = np.random.random(size=(64 * 64 * 32, 3))
+    points_log_odds = test_map.interpolateTrilinear(points)
+    for point_idx in range(points.shape[0]):
+        point = points[point_idx, :]
+        point_log_odds = test_map.interpolateTrilinear(point)
+        assert points_log_odds[point_idx] == point_log_odds
diff --git a/tooling/git_hook_configs/.pylintrc b/tooling/git_hook_configs/.pylintrc
index 60115acba..7bae290d6 100644
--- a/tooling/git_hook_configs/.pylintrc
+++ b/tooling/git_hook_configs/.pylintrc
@@ -78,17 +78,7 @@ confidence=
 # --enable=similarities". If you want to run only the classes checker, but have
 # no Warning level messages displayed, use "--disable=all --enable=classes
 # --disable=W".
-disable=print-statement,
-        parameter-unpacking,
-        unpacking-in-except,
-        old-raise-syntax,
-        backtick,
-        long-suffix,
-        old-ne-operator,
-        old-octal-literal,
-        import-star-module-level,
-        non-ascii-bytes-literal,
-        raw-checker-failed,
+disable=raw-checker-failed,
         bad-inline-option,
         locally-disabled,
         file-ignored,
@@ -96,67 +86,6 @@ disable=print-statement,
         useless-suppression,
         deprecated-pragma,
         use-symbolic-message-instead,
-        apply-builtin,
-        basestring-builtin,
-        buffer-builtin,
-        cmp-builtin,
-        coerce-builtin,
-        execfile-builtin,
-        file-builtin,
-        long-builtin,
-        raw_input-builtin,
-        reduce-builtin,
-        standarderror-builtin,
-        unicode-builtin,
-        xrange-builtin,
-        coerce-method,
-        delslice-method,
-        getslice-method,
-        setslice-method,
-        no-absolute-import,
-        old-division,
-        dict-iter-method,
-        dict-view-method,
-        next-method-called,
-        metaclass-assignment,
-        indexing-exception,
-        raising-string,
-        reload-builtin,
-        oct-method,
-        hex-method,
-        nonzero-method,
-        cmp-method,
-        input-builtin,
-        round-builtin,
-        intern-builtin,
-        unichr-builtin,
-        map-builtin-not-iterating,
-        zip-builtin-not-iterating,
-        range-builtin-not-iterating,
-        filter-builtin-not-iterating,
-        using-cmp-argument,
-        eq-without-hash,
-        div-method,
-        idiv-method,
-        rdiv-method,
-        exception-message-attribute,
-        invalid-str-codec,
-        sys-max-int,
-        bad-python3-import,
-        deprecated-string-function,
-        deprecated-str-translate-call,
-        deprecated-itertools-function,
-        deprecated-types-field,
-        next-method-defined,
-        dict-items-not-iterating,
-        dict-keys-not-iterating,
-        dict-values-not-iterating,
-        deprecated-operator-function,
-        deprecated-urllib-function,
-        xreadlines-attribute,
-        deprecated-sys-function,
-        exception-escape,
-        comprehension-escape,  # AMZ: Added after this line
         missing-module-docstring,
         missing-class-docstring,
         missing-function-docstring,
@@ -653,5 +582,5 @@ valid-metaclass-classmethod-first-arg=cls
 
 # Exceptions that will emit a warning when being caught. Defaults to
 # "BaseException, Exception".
-overgeneral-exceptions=BaseException,
-                       Exception
+overgeneral-exceptions=builtins.BaseException,
+                       builtins.Exception

From 6225fee2ed61c18fb7f96c8ac9e4854cc5b8cef6 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 3 Sep 2024 12:35:20 +0200
Subject: [PATCH 50/85] Warn user and ignore range images of wrong dimensions
 to avoid segfault

---
 .../integrator/projective/projective_integrator.cc  | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/library/cpp/src/core/integrator/projective/projective_integrator.cc b/library/cpp/src/core/integrator/projective/projective_integrator.cc
index acacc2003..b61b169d5 100644
--- a/library/cpp/src/core/integrator/projective/projective_integrator.cc
+++ b/library/cpp/src/core/integrator/projective/projective_integrator.cc
@@ -1,5 +1,6 @@
 #include "wavemap/core/integrator/projective/projective_integrator.h"
 
+#include <wavemap/core/utils/data/eigen_checks.h>
 #include <wavemap/core/utils/profiler_interface.h>
 
 namespace wavemap {
@@ -32,6 +33,15 @@ void ProjectiveIntegrator::integrate(const PosedPointcloud<>& pointcloud) {
 
 void ProjectiveIntegrator::integrate(const PosedImage<>& range_image) {
   ProfilerZoneScoped;
+  CHECK_NOTNULL(projection_model_);
+  if (range_image.getDimensions() != projection_model_->getDimensions()) {
+    LOG(WARNING) << "Dimensions of range image"
+                 << print::eigen::oneLine(range_image.getDimensions())
+                 << " do not match projection model"
+                 << print::eigen::oneLine(projection_model_->getDimensions())
+                 << ". Ignoring integration request.";
+    return;
+  }
   if (!isPoseValid(range_image.getPose())) {
     return;
   }
@@ -81,6 +91,9 @@ void ProjectiveIntegrator::importPointcloud(
 void ProjectiveIntegrator::importRangeImage(
     const PosedImage<>& range_image_input) {
   ProfilerZoneScoped;
+  CHECK_NOTNULL(posed_range_image_);
+  CHECK_EIGEN_EQ(range_image_input.getDimensions(),
+                 posed_range_image_->getDimensions());
   *posed_range_image_ = range_image_input;
   beam_offset_image_->resetToInitialValue();
 }

From 3c57888afaa09cd2af96e31edc870c5a97e77640 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 4 Sep 2024 13:34:42 +0200
Subject: [PATCH 51/85] Update how we refer to GH Issues and Discussions in the
 docs

---
 .github/ISSUE_TEMPLATE/question.md | 44 ------------------------------
 docs/pages/contributing.rst        | 13 +++++++--
 docs/pages/faq.rst                 | 11 +++-----
 docs/pages/tutorials/python.rst    |  4 +++
 4 files changed, 19 insertions(+), 53 deletions(-)
 delete mode 100644 .github/ISSUE_TEMPLATE/question.md

diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md
deleted file mode 100644
index bb507ee59..000000000
--- a/.github/ISSUE_TEMPLATE/question.md
+++ /dev/null
@@ -1,44 +0,0 @@
----
-name: Question
-about: Ask a general question
-title: ''
-labels:
-assignees: victorreijgwart
-
----
-
-**Question**
-A clear and concise description of your question.
-
-**Existing resources**
-Before submitting this question, please check if it has been answered in a previous [GitHub Issue](https://github.com/ethz-asl/wavemap/issues?q=is%3Aissue).
-
-If the question is about the theory behind wavemap, it might already be addressed in our [RSS paper](https://www.roboticsproceedings.org/rss19/p065.pdf).
-
-If you would like to reproduce the results from our [RSS paper](https://www.roboticsproceedings.org/rss19/p065.pdf), please refer to the [Demo](https://ethz-asl.github.io/wavemap/pages/demos.html) documentation page.
-
-If you need help configuring wavemap on a custom dataset, the [Configuration](https://ethz-asl.github.io/wavemap/pages/configuration.html) documentation page might be helpful.
-
-**Images**
-If it helps to explain your question, please include screenshots, plots or sketches.
-
-**Runtime information:**
-Please fill out these questions in case a specific dataset or sensor setup is relevant to your question.
-
-- Launch file: [e.g. Link to the launch file you used]
-- Config file: [e.g. Link to the config file you used]
-- Dataset name [e.g. Newer College Cloister sequence]  # For public datasets
-- Custom setup:                                        # For online use or personal datasets
-    - depth sensor: [e.g. Livox MID360 LiDAR]
-    - pose source: [e.g. Odometry from FastLIO2]
-
-**System information:**
-Please fill out these questions in case your hardware is relevant to your question. For example, if you would like help to tune wavemap's performance on your robot.
-
-- CPU: [e.g. Intel i9-9900K]
-- GPU: [e.g. Nvidia RTX 2080Ti]  # Only for visualization-related issues
-- RAM: [e.g. 32GB]
-- OS: [e.g. Ubuntu 20.04]
-- Wavemap
-    - install: [e.g., Native (ROS with catkin); or Docker]
-    - version: [e.g., v1.4.0]
diff --git a/docs/pages/contributing.rst b/docs/pages/contributing.rst
index 1a1891aba..def20104e 100644
--- a/docs/pages/contributing.rst
+++ b/docs/pages/contributing.rst
@@ -3,11 +3,20 @@ Contributing
 .. highlight:: bash
 .. rstcheck: ignore-roles=gh_file
 
-Thank you for investing time in contributing to wavemap!
+Thank you for your interest in contributing to wavemap!
+
+Questions
+*********
+If you have any questions, feel free to ask them in the `Q&A section <https://github.com/ethz-asl/wavemap/discussions/categories/q-a>`_ of our GitHub Discussions.
+
+Before posting, please check if your question has already been addressed in our :doc:`installation <installation/index>` or :doc:`usage <tutorials/index>` tutorials. We're happy to answer any remaining theoretical or code-related questions, and help you optimize wavemap for your specific sensor setup.
 
 Bug reports & Feature requests
 ******************************
-We welcome bug reports, feature requests, and general questions. Please submit them through `GitHub Issues <https://github.com/ethz-asl/wavemap/issues>`_ and use the corresponding `bug report <https://github.com/ethz-asl/wavemap/issues/new?template=bug_report.md>`_, `feature request <https://github.com/ethz-asl/wavemap/issues/new?template=feature_request.md>`_, and `question <https://github.com/ethz-asl/wavemap/issues/new?template=question.md>`_ templates.
+We encourage you to submit bug reports and feature requests. You can do so using the relevant GitHub Issue templates:
+
+* `Bug report <https://github.com/ethz-asl/wavemap/issues/new?template=bug_report.md>`_
+* `Feature request <https://github.com/ethz-asl/wavemap/issues/new?template=feature_request.md>`_
 
 In addition to requests for new functionality, do not hesitate to open feature requests for:
 
diff --git a/docs/pages/faq.rst b/docs/pages/faq.rst
index be4070e4f..9b8da7106 100644
--- a/docs/pages/faq.rst
+++ b/docs/pages/faq.rst
@@ -1,12 +1,9 @@
 FAQ
 ###
+For a comprehensive list of frequently asked questions, please visit the `FAQ section <https://github.com/ethz-asl/wavemap/discussions/categories/faq>`_ of our GitHub Discussions.
 
-If you have a question that is not yet answered below, feel free to open a `GitHub Issue <https://github.com/ethz-asl/wavemap/issues>`_ or contact us over email.
+Many practical questions may also be covered in our :doc:`Installation <installation/index>` and :doc:`Usage <tutorials/index>` tutorials, which include a variety of code examples.
 
-How do I query if a point in the map is occupied?
-=================================================
-Please see the :doc:`usage examples <tutorials/cpp>` on :ref:`interpolation <cpp-code-examples-interpolation>` and :ref:`classification <cpp-code-examples-classification>`.
+For a theoretical introduction to the concepts behind wavemap, you can explore our open-access RSS paper, available for download `here <https://www.roboticsproceedings.org/rss19/p065.pdf>`_, and summarized in `this 5-minute presentation <https://www.youtube.com/watch?v=ftQhK75Ri1E&t=733s>`_.
 
-Does wavemap support (Euclidean) Signed Distance Fields?
-========================================================
-Not yet, but we will add this feature in the near future.
+If your question remains unanswered, don't hesitate to ask in the `Q&A section <https://github.com/ethz-asl/wavemap/discussions/categories/q-a>`_ of our GitHub Discussions. We’d be happy to assist with any remaining theoretical or code-related questions, and help you optimize wavemap for your sensor setup.
diff --git a/docs/pages/tutorials/python.rst b/docs/pages/tutorials/python.rst
index 75b340ccb..dde65c4e8 100644
--- a/docs/pages/tutorials/python.rst
+++ b/docs/pages/tutorials/python.rst
@@ -108,6 +108,8 @@ If you need to look up multiple node values, we recommend using our batched quer
 
     So far batched queries are only implemented for HashedWaveletOctree maps. We will add support for HashedChunkedWaveletOctree maps in the near future.
 
+.. _python-code-examples-interpolation:
+
 Real coordinates
 ----------------
 Many applications require occupancy estimates at arbitrary 3D points, with real-valued coordinates. Such estimates are computed by interpolating the map.
@@ -130,6 +132,8 @@ Another option is to linearly interpolate the map along the x, y, and z axes. Th
 .. literalinclude:: ../../../examples/python/queries/trilinear_interpolation.py
     :language: python
 
+.. _python-code-examples-classification:
+
 Occupancy classification
 ------------------------
 Once the estimated occupancy at a node or point has been retrieved, it can be classified as follows.

From 2143b96297264887e5d9767949c823892de5d3b1 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 4 Sep 2024 14:32:58 +0200
Subject: [PATCH 52/85] Start refactoring CI pipeline

---
 .github/workflows/cd.yml   | 233 ---------------
 .github/workflows/ci.yml   | 571 -------------------------------------
 .github/workflows/lint.yml |  53 ++++
 3 files changed, 53 insertions(+), 804 deletions(-)
 delete mode 100644 .github/workflows/cd.yml
 delete mode 100644 .github/workflows/ci.yml
 create mode 100644 .github/workflows/lint.yml

diff --git a/.github/workflows/cd.yml b/.github/workflows/cd.yml
deleted file mode 100644
index 226b1fe37..000000000
--- a/.github/workflows/cd.yml
+++ /dev/null
@@ -1,233 +0,0 @@
-name: Continuous Deployment
-
-on:
-  push:
-    tags:
-      - "v*.*.*"
-    branches: [ main ]
-
-# NOTE: We do not store the work files under $HOME ("/github/home/") since that
-#       dir persists between jobs when using self-hosted GitHub Actions runners
-#       (/github/home is a docker volume mapped to the container's host).
-env:
-  REPOSITORY_NAME: wavemap
-  DOCKER_CI_REGISTRY: hub.wavemap.vwire.ch
-  DOCKER_RELEASE_REGISTRY: ghcr.io
-  DOCKER_RELEASE_TARGET: workspace
-  USER_HOME: /home/ci
-  CATKIN_WS_PATH: /home/ci/catkin_ws
-  CCACHE_DIR: /home/ci/ccache
-
-jobs:
-  common-variables:
-    name: Define common variables
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: docker:20.10.9-dind
-    outputs:
-      docker_cache_image_name: type=registry,ref=${{ env.DOCKER_CI_REGISTRY }}/${{ env.REPOSITORY_NAME }}:buildcache
-      local_ci_image_name: ${{ env.DOCKER_CI_REGISTRY }}/${{ env.REPOSITORY_NAME }}:${{ env.DOCKER_RELEASE_TARGET }}-${{ github.sha }}
-    steps:
-      - name: Empty
-        run: echo
-
-  draft-release:
-    name: Draft Release
-    if: startsWith(github.event.ref, 'refs/tags/v')
-    runs-on: ubuntu-latest
-    steps:
-      - name: Checkout code
-        uses: actions/checkout@v4
-      - name: Create Release
-        id: create_release
-        uses: softprops/action-gh-release@v1
-
-  build-image:
-    name: Build Docker image
-    needs: [ common-variables ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: docker:20.10.9-dind
-    permissions:
-      contents: read
-      packages: write
-    outputs:
-      image: ${{ needs.common-variables.outputs.local_ci_image_name }}
-    env:
-      CACHE_IMAGE_NAME: ${{ needs.common-variables.outputs.docker_cache_image_name }}
-      LOCAL_IMAGE_NAME: ${{ needs.common-variables.outputs.local_ci_image_name }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        with:
-          path: ${{ env.REPOSITORY_NAME }}
-
-      - name: Log in to the Container registry
-        uses: docker/login-action@65b78e6e13532edd9afa3aa52ac7964289d1a9c1
-        with:
-          registry: ${{ env.DOCKER_RELEASE_REGISTRY }}
-          username: ${{ github.actor }}
-          password: ${{ secrets.GITHUB_TOKEN }}
-
-      - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
-
-      - name: Build the ${{ env.DOCKER_RELEASE_TARGET }} image
-        uses: docker/build-push-action@v6
-        with:
-          context: ${{ env.REPOSITORY_NAME }}
-          file: ${{ env.REPOSITORY_NAME }}/tooling/docker/ros1/full.Dockerfile
-          target: ${{ env.DOCKER_RELEASE_TARGET }}
-          build-args: |
-            REPOSITORY_NAME=${{ env.REPOSITORY_NAME }}
-            USER_HOME=${{ env.USER_HOME }}
-            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
-            CCACHE_DIR=${{ env.CCACHE_DIR }}
-          load: true
-          cache-from: ${{ env.CACHE_IMAGE_NAME }}
-          cache-to: ${{ env.CACHE_IMAGE_NAME }},mode=max
-          tags: ${{ env.LOCAL_IMAGE_NAME }}
-
-      - name: Test the ${{ env.DOCKER_RELEASE_TARGET }} image
-        run: docker run --rm ${{ env.LOCAL_IMAGE_NAME }}
-
-      - name: Push the ${{ env.DOCKER_RELEASE_TARGET }} image locally
-        uses: docker/build-push-action@v6
-        with:
-          context: ${{ env.REPOSITORY_NAME }}
-          file: ${{ env.REPOSITORY_NAME }}/tooling/docker/ros1/full.Dockerfile
-          target: ${{ env.DOCKER_RELEASE_TARGET }}
-          build-args: |
-            REPOSITORY_NAME=${{ env.REPOSITORY_NAME }}
-            USER_HOME=${{ env.USER_HOME }}
-            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
-            CCACHE_DIR=${{ env.CCACHE_DIR }}
-          push: true
-          cache-from: ${{ env.CACHE_IMAGE_NAME }}
-          tags: ${{ env.LOCAL_IMAGE_NAME }}
-
-      - name: Extract metadata to annotate the image
-        id: meta
-        uses: docker/metadata-action@9ec57ed1fcdbf14dcef7dfbe97b2010124a938b7
-        with:
-          images: ${{ env.DOCKER_RELEASE_REGISTRY }}/${{ github.repository }}_ros1
-
-      - name: Publish the ${{ env.DOCKER_RELEASE_TARGET }} image
-        if: startsWith(github.event.ref, 'refs/tags/v')
-        uses: docker/build-push-action@v6
-        with:
-          context: ${{ env.REPOSITORY_NAME }}
-          file: ${{ env.REPOSITORY_NAME }}/tooling/docker/ros1/full.Dockerfile
-          target: ${{ env.DOCKER_RELEASE_TARGET }}
-          build-args: |
-            REPOSITORY_NAME=${{ env.REPOSITORY_NAME }}
-            USER_HOME=${{ env.USER_HOME }}
-            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
-            CCACHE_DIR=${{ env.CCACHE_DIR }}
-          push: true
-          cache-from: ${{ env.CACHE_IMAGE_NAME }}
-          tags: ${{ steps.meta.outputs.tags }}
-          labels: ${{ steps.meta.outputs.labels }}
-
-  build-docs:
-    name: Build docs
-    needs: [ build-image ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.build-image.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-
-      - name: Install dependencies (doxygen+sphinx+breathe+exhale toolchain)
-        run: |
-          apt-get update
-          apt-get install -q -y --no-install-recommends python3-pip doxygen
-          apt-get install -q -y --no-install-recommends latexmk texlive-latex-extra tex-gyre texlive-fonts-recommended texlive-latex-recommended
-          pip3 install exhale sphinx-sitemap sphinx-design sphinx-notfound-page
-          pip3 install sphinxawesome-theme --pre
-          pip3 install "sphinx<7,>6"
-
-      - name: Parse C++ library with Doxygen
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: doxygen Doxyfile_cpp
-
-      - name: Parse ROS1 interface with Doxygen
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: doxygen Doxyfile_ros1
-
-      - name: Build documentation site
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: sphinx-build -b html . _build/html
-
-      - name: Bundle site sources into tarball
-        shell: bash
-        run: |
-          tar \
-            --dereference --hard-dereference \
-            --directory ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/_build/html/ \
-            -cvf ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/artifact.tar \
-            --exclude=.git \
-            --exclude=.github \
-            .
-
-      - name: Upload tarball as GH Pages artifact
-        uses: actions/upload-artifact@v3
-        with:
-          name: github-pages
-          path: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/artifact.tar
-          retention-days: 1
-
-      - name: Build documentation PDF
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: sphinx-build -M latexpdf . _build/latex
-
-      - name: Attach PDF to GitHub release
-        if: startsWith(github.event.ref, 'refs/tags/v')
-        uses: actions/github-script@v6
-        with:
-          script: |
-            const fs = require('fs');
-            const tag = context.ref.replace("refs/tags/", "");
-            // Get release for this tag
-            const release = await github.rest.repos.getReleaseByTag({
-              owner: context.repo.owner,
-              repo: context.repo.repo,
-              tag
-            });
-            // Upload the release asset
-            await github.rest.repos.uploadReleaseAsset({
-              owner: context.repo.owner,
-              repo: context.repo.repo,
-              release_id: release.data.id,
-              name: "docs.pdf",
-              data: await fs.readFileSync("${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/_build/latex/latex/wavemap.pdf")
-            });
-
-  publish-docs:
-    name: Publish docs
-    needs: [ build-docs ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: docker:20.10.9-dind
-    permissions:
-      contents: read
-      pages: write
-      id-token: write
-    concurrency:
-      group: "pages"
-      cancel-in-progress: true
-    environment:
-      name: github-pages
-      url: ${{ steps.deployment.outputs.page_url }}
-    steps:
-      - name: Setup Pages
-        uses: actions/configure-pages@v4
-
-      - name: Deploy uploaded docs to GitHub Pages
-        id: deployment
-        uses: actions/deploy-pages@v3
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
deleted file mode 100644
index 66d30d185..000000000
--- a/.github/workflows/ci.yml
+++ /dev/null
@@ -1,571 +0,0 @@
-name: Continuous Integration
-
-on:
-  pull_request:
-    branches: [ main ]
-
-# NOTE: We do not store the work files under $HOME ("/github/home/") since that
-#       dir persists between jobs when using self-hosted GitHub Actions runners
-#       (/github/home is a docker volume mapped to the container's host).
-env:
-  REPOSITORY_NAME: wavemap
-  DOCKER_CI_REGISTRY: hub.wavemap.vwire.ch
-  DOCKER_CI_TARGET: workspace
-  USER_HOME: /home/ci
-  CATKIN_WS_PATH: /home/ci/catkin_ws
-  CCACHE_DIR: /home/ci/ccache
-  PRE_COMMIT_DIR: /home/ci/pre-commit
-
-jobs:
-  common-variables:
-    name: Define common variables
-    # NOTE: This job is used to pass complex common variables around between jobs,
-    #       as a work-around for ENV variables in GitHub Actions not being composable.
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: docker:20.10.9-dind
-    outputs:
-      docker_cache_image_name: type=registry,ref=${{ env.DOCKER_CI_REGISTRY }}/${{ env.REPOSITORY_NAME }}:buildcache
-      local_ci_image_name: ${{ env.DOCKER_CI_REGISTRY }}/${{ env.REPOSITORY_NAME }}:${{ env.DOCKER_CI_TARGET }}-${{ github.sha }}
-    steps:
-      - name: Empty
-        run: echo
-
-  workspace-container:
-    name: Build CI workspace container
-    needs: [ common-variables ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: docker:20.10.9-dind
-    outputs:
-      image: ${{ needs.common-variables.outputs.local_ci_image_name }}
-    env:
-      CACHE_IMAGE_NAME: ${{ needs.common-variables.outputs.docker_cache_image_name }}
-      LOCAL_IMAGE_NAME: ${{ needs.common-variables.outputs.local_ci_image_name }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        with:
-          path: ${{ env.REPOSITORY_NAME }}
-
-      - name: Install dependencies
-        # NOTE: Installing tar is required for actions/cache@v4 to work properly
-        #       on docker:20.10.9-dind.
-        run: apk add --no-cache tar git
-
-      - name: Set up Docker Buildx
-        uses: docker/setup-buildx-action@v3
-
-      - name: Build the ${{ env.DOCKER_CI_TARGET }} image
-        uses: docker/build-push-action@v6
-        with:
-          context: ${{ env.REPOSITORY_NAME }}
-          file: ${{ env.REPOSITORY_NAME }}/tooling/docker/ros1/full.Dockerfile
-          target: ${{ env.DOCKER_CI_TARGET }}
-          build-args: |
-            REPOSITORY_NAME=${{ env.REPOSITORY_NAME }}
-            USER_HOME=${{ env.USER_HOME }}
-            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
-            CCACHE_DIR=${{ env.CCACHE_DIR }}
-          load: true
-          cache-from: ${{ env.CACHE_IMAGE_NAME }}
-          cache-to: ${{ env.CACHE_IMAGE_NAME }},mode=max
-          tags: ${{ env.LOCAL_IMAGE_NAME }}
-
-      - name: Test the ${{ env.DOCKER_CI_TARGET }} image
-        run: docker run --rm ${{ env.LOCAL_IMAGE_NAME }}
-
-      - name: Push the ${{ env.DOCKER_CI_TARGET }} image
-        uses: docker/build-push-action@v6
-        with:
-          context: ${{ env.REPOSITORY_NAME }}
-          file: ${{ env.REPOSITORY_NAME }}/tooling/docker/ros1/full.Dockerfile
-          target: ${{ env.DOCKER_CI_TARGET }}
-          build-args: |
-            REPOSITORY_NAME=${{ env.REPOSITORY_NAME }}
-            USER_HOME=${{ env.USER_HOME }}
-            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
-            CCACHE_DIR=${{ env.CCACHE_DIR }}
-          push: true
-          cache-from: ${{ env.CACHE_IMAGE_NAME }}
-          tags: ${{ env.LOCAL_IMAGE_NAME }}
-
-  lint:
-    name: Lint
-    needs: [ common-variables ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      # NOTE: Pylint checks if all modules that are marked for import are
-      #       available. At the time of writing, the python scripts in this repo
-      #       only depend on modules that are present on noetic-ros-base-focal
-      #       out of the box. If scripts are added later that depend on custom
-      #       package (e.g. installed through rosdep or pulled in through
-      #       vcstool), it'd make sense to run pre-commit in a full workspace
-      #       container (such as ${{ needs.workspace-container.outputs.image }})
-      #       at the cost of a longer loading time on the CI actions runner.
-      image: ros:noetic-ros-base-focal
-    steps:
-      - name: Install pre-commit's dependencies
-        run: |
-          apt-get update
-          apt-get install -q -y --no-install-recommends git python3-pip clang-format-11 cppcheck libxml2-utils wget
-          pip3 install pre-commit cpplint
-          wget -O /bin/hadolint https://github.com/hadolint/hadolint/releases/download/v2.8.0/hadolint-Linux-x86_64
-          chmod +x /bin/hadolint
-
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: This has to be done after installing pre-commit, s.t. the
-        #       pre-commit hooks are automatically initialized.
-
-      - name: Get python version for pre-commit cache
-        run: echo "PRE_COMMIT_PYTHON_VERSION=$(python -VV | sha256sum | cut -d' ' -f1)" >> $GITHUB_ENV
-
-      - name: Setup pre-commit cache sharing
-        uses: actions/cache@v4
-        with:
-          path: ${{ env.PRE_COMMIT_DIR }}
-          key: pre-commit|${{ env.PRE_COMMIT_PYTHON_VERSION }}|${{ hashFiles('.pre-commit-config.yaml') }}
-
-      - name: Run the pre-commit hooks
-        shell: bash
-        run: |
-          echo "::add-matcher::./.github/problem-matchers/pre-commit.json"
-          source /opt/ros/noetic/setup.bash
-          PRE_COMMIT_HOME=${{ env.PRE_COMMIT_DIR }} SKIP=no-commit-to-branch pre-commit run --all-files
-          echo "::remove-matcher owner=problem-matcher-pre-commit::"
-
-  build-docs:
-    name: Build docs
-    needs: [ workspace-container, lint ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-
-      - name: Install dependencies (doxygen+sphinx+breathe+exhale toolchain)
-        run: |
-          apt-get update
-          apt-get install -q -y --no-install-recommends python3-pip doxygen
-          apt-get install -q -y --no-install-recommends latexmk texlive-latex-extra tex-gyre texlive-fonts-recommended texlive-latex-recommended
-          pip3 install exhale sphinx-sitemap sphinx-design sphinx-notfound-page
-          pip3 install sphinxawesome-theme --pre
-          pip3 install "sphinx<7,>6"
-
-      - name: Parse C++ library with Doxygen
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: doxygen Doxyfile_cpp
-
-      - name: Parse ROS1 interface with Doxygen
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: doxygen Doxyfile_ros1
-
-      - name: Build documentation site
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: sphinx-build -b html . _build/html
-
-      - name: Bundle site sources into tarball
-        shell: bash
-        run: |
-          tar \
-            --dereference --hard-dereference \
-            --directory ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/_build/html/ \
-            -cvf ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/artifact.tar \
-            --exclude=.git \
-            --exclude=.github \
-            .
-
-      - name: Upload tarball as GH Pages artifact
-        uses: actions/upload-artifact@main
-        with:
-          name: github-pages
-          path: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/artifact.tar
-          retention-days: 1
-
-      - name: Build documentation PDF
-        working-directory: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs
-        shell: bash
-        run: sphinx-build -M latexpdf . _build/latex
-
-      - name: Upload PDF
-        uses: actions/upload-artifact@main
-        with:
-          name: documentation-pdf
-          path: ${{ env.CATKIN_WS_PATH }}/src/${{ env.REPOSITORY_NAME }}/docs/_build/latex/latex/wavemap.pdf
-          retention-days: 3
-
-  build:
-    name: Build
-    needs: workspace-container
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: Even though the repo is already present in the container, we
-        #       also need to check it out at GitHub Actions' preferred location
-        #       for private actions and problem matchers to work.
-
-      - name: Setup ccache
-        uses: ./.github/actions/setup-ccache
-        with:
-          cache-group: noetic-gcc-release
-          cache-version: ${{ secrets.CCACHE_CACHE_VERSION }}
-
-      - name: Build all wavemap packages
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          echo "::add-matcher::./.github/problem-matchers/gcc.json"
-          catkin build wavemap_all --no-status --force-color
-          echo "::remove-matcher owner=problem-matcher-gcc::"
-
-      - name: Show statistics for ccache
-        uses: ./.github/actions/log-ccache-stats
-
-  install:
-    name: Catkin install
-    needs: [ workspace-container, build ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: Even though the repo is already present in the container, we
-        #       also need to check it out at GitHub Actions' preferred location
-        #       for private actions and problem matchers to work.
-
-      - name: Setup ccache
-        uses: ./.github/actions/setup-ccache
-        with:
-          cache-group: noetic-gcc-release
-          cache-version: ${{ secrets.CCACHE_CACHE_VERSION }}
-
-      - name: Enable catkin install
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          catkin config --install
-          catkin clean -b -y
-
-      - name: Build all wavemap packages
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          . /opt/ros/noetic/setup.sh
-          echo "::add-matcher::./.github/problem-matchers/gcc.json"
-          catkin build wavemap_all --no-status --force-color
-          echo "::remove-matcher owner=problem-matcher-gcc::"
-
-      - name: Show statistics for ccache
-        uses: ./.github/actions/log-ccache-stats
-
-  clang-tidy:
-    name: Clang tidy
-    needs: [ workspace-container, build ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: Even though the repo is already present in the container, we
-        #       also need to check it out at GitHub Actions' preferred location
-        #       for private actions and problem matchers to work.
-
-      - name: Setup ccache
-        uses: ./.github/actions/setup-ccache
-        with:
-          cache-group: noetic-gcc-release
-          cache-version: ${{ secrets.CCACHE_CACHE_VERSION }}
-
-      - name: Install clang-tidy
-        run: |
-          apt-get update
-          apt-get install -q -y --no-install-recommends clang-tidy
-
-      - name: Build catkin package and dependencies
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: catkin build wavemap_all --no-status --force-color --cmake-args -DUSE_CLANG_TIDY=ON
-
-      - name: Run clang-tidy for wavemap
-        working-directory: ${{ env.CATKIN_WS_PATH }}/build/wavemap
-        run: |
-          echo "::add-matcher::./.github/problem-matchers/clang-tidy.json"
-          run-clang-tidy -header-filter="*include/wavemap/*" -quiet
-          echo "::remove-matcher owner=problem-matcher-clang-tidy::"
-
-      - name: Run clang-tidy for wavemap_ros
-        working-directory: ${{ env.CATKIN_WS_PATH }}/build/wavemap_ros
-        run: |
-          echo "::add-matcher::./.github/problem-matchers/clang-tidy.json"
-          run-clang-tidy -header-filter="*include/wavemap_ros/*" -quiet
-          echo "::remove-matcher owner=problem-matcher-clang-tidy::"
-
-  test:
-    name: Test
-    needs: [ workspace-container, build ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: Even though the repo is already present in the container, we
-        #       also need to check it out at GitHub Actions' preferred location
-        #       for private actions and problem matchers to work.
-
-      - name: Setup ccache
-        uses: ./.github/actions/setup-ccache
-        with:
-          cache-group: noetic-gcc-release
-          cache-version: ${{ secrets.CCACHE_CACHE_VERSION }}
-
-      - name: Build regular code
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: catkin build wavemap_all --no-status --force-color --cmake-args -DDCHECK_ALWAYS_ON=ON
-
-      - name: Build unit tests
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          echo "::add-matcher::./.github/problem-matchers/gcc.json"
-          catkin build wavemap_all --no-status --force-color --no-deps --cmake-args -DDCHECK_ALWAYS_ON=ON --catkin-make-args tests
-          echo "::remove-matcher owner=problem-matcher-gcc::"
-
-      - name: Run unit tests
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          all_tests_passed=1
-          source devel/setup.bash
-          for f in devel/lib/wavemap*/test_*
-            do $f --gtest_color=yes || all_tests_passed=0
-          done
-          if [ $all_tests_passed -ne 1 ]; then
-            echo "Not all tests passed!"
-            exit 1
-          fi
-
-      - name: Show statistics for ccache
-        uses: ./.github/actions/log-ccache-stats
-
-  coverage:
-    name: Coverage
-    # TODO(victorr): Enable this again once it has been updated to work with the new package structure
-    if: ${{ false }}
-    needs: [ workspace-container, test ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: Even though the repo is already present in the container, we
-        #       also need to check it out at GitHub Actions' preferred location
-        #       for private actions and problem matchers to work.
-
-      - name: Setup ccache
-        uses: ./.github/actions/setup-ccache
-        with:
-          cache-group: noetic-gcc-debug
-          cache-version: ${{ secrets.CCACHE_CACHE_VERSION }}
-
-      - name: Install lcov for coverage report generation
-        run: |
-          apt-get update
-          apt-get install -q -y --no-install-recommends lcov
-
-      - name: Switch catkin workspace to debug mode
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          catkin clean -y
-          catkin config --cmake-args -DCMAKE_BUILD_TYPE=Debug
-
-      - name: Rebuild dependencies and build regular code (in debug mode)
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          source /opt/ros/noetic/setup.bash
-          catkin build wavemap_all --no-status --force-color
-
-      - name: Build unit tests
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: catkin build wavemap_all --no-status --force-color --no-deps --cmake-args -DENABLE_COVERAGE_TESTING=ON --catkin-make-args tests
-
-      - name: Set coverage counters to zero and create report base
-        working-directory: ${{ env.CATKIN_WS_PATH }}/build/wavemap
-        shell: bash
-        run: |
-          lcov --zerocounters --directory .
-          lcov --capture --initial --directory . --output-file wavemap_coverage_base.info
-
-      - name: Run all tests while measuring coverage
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          all_tests_passed=1
-          for f in devel/lib/wavemap*/test_*
-          do
-            $f --gtest_color=yes || all_tests_passed=0
-          done
-          if [ $all_tests_passed -ne 1 ]; then
-            echo "Not all tests passed! Note that the code is currently compiled"\
-                 "in Debug mode, so some additional errors may be caught compared"\
-                 "to previous test runs in Release mode (e.g. failing DCHECKs)."
-            exit 1
-          fi
-
-      - name: Create the coverage report
-        working-directory: ${{ env.CATKIN_WS_PATH }}/build/wavemap
-        shell: bash
-        run: |
-          lcov --capture --directory . --output-file wavemap_coverage_unit_tests.info
-          lcov --add-tracefile wavemap_coverage_base.info --add-tracefile wavemap_coverage_unit_tests.info --output-file wavemap_coverage_total.info
-          lcov --extract wavemap_coverage_total.info '*/wavemap/wavemap*' --output-file wavemap_coverage_filtered_intermediate.info
-          lcov --remove wavemap_coverage_filtered_intermediate.info '*/wavemap/test/*' '*/wavemap/app/*' '*/wavemap/benchmark/*' --output-file wavemap_coverage.info
-          rm wavemap_coverage_base.info wavemap_coverage_unit_tests.info wavemap_coverage_total.info wavemap_coverage_filtered_intermediate.info
-          lcov --list wavemap_coverage.info  # Include report in logs for debugging
-
-      - name: Upload coverage stats to Codecov
-        uses: codecov/codecov-action@v2
-        with:
-          token: ${{ secrets.CODECOV_TOKEN }}
-          directory: ${{ env.CATKIN_WS_PATH }}/build/wavemap
-          flags: unittests
-          fail_ci_if_error: true
-          verbose: true
-
-      - name: Show statistics for ccache
-        uses: ./.github/actions/log-ccache-stats
-
-  sanitize:
-    name: Sanitize ${{ matrix.sanitizer.detects }}
-    needs: [ workspace-container, test ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    strategy:
-      matrix:
-        sanitizer:
-          - { name: UBSAN, detects: 'undefined behavior' }
-          - { name: ASAN, detects: 'addressability and leaks' }
-          # - { name: TSAN, detects: 'data races and deadlocks' }
-          # NOTE: TSAN is disabled until the following bug is resolved:
-          #       https://bugs.launchpad.net/ubuntu/+source/gcc-10/+bug/2029910.
-          # NOTE: MSAN is not used for now since it also requires all deps to be
-          #       instrumented (recompiled with clang and the MSan flags, LLVM's
-          #       stdlib instead of GCCs,...). We therefore use Valgrind to
-          #       check for uninitialized memory usage errors instead.
-      fail-fast: false
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: Even though the repo is already present in the container, we
-        #       also need to check it out at GitHub Actions' preferred location
-        #       for private actions and problem matchers to work.
-
-      - name: Setup ccache
-        uses: ./.github/actions/setup-ccache
-        with:
-          cache-group: noetic-gcc-release
-          cache-version: ${{ secrets.CCACHE_CACHE_VERSION }}
-
-      - name: Build regular code
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: catkin build wavemap_all --no-status --force-color
-
-      - name: Build unit tests
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: catkin build wavemap_all --no-status --force-color --no-deps --cmake-args -DUSE_${{ matrix.sanitizer.name }}=ON --catkin-make-args tests
-
-      - name: Check unit tests with ${{ matrix.sanitizer.name }}
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        env:
-          UBSAN_OPTIONS: halt_on_error=1:print_stacktrace=1
-          ASAN_OPTIONS: halt_on_error=1:detect_leaks=1:detect_stack_use_after_return=1
-          TSAN_OPTIONS: halt_on_error=1:second_deadlock_stack=1
-        shell: bash
-        run: |
-          echo "::add-matcher::./.github/problem-matchers/gcc-sanitizers.json"
-          all_tests_passed=1
-          for f in devel/lib/wavemap*/test_*
-            do $f --gtest_color=yes || all_tests_passed=0
-          done
-          if [ $all_tests_passed -ne 1 ]; then
-            echo "Not all tests passed!"
-            exit 1
-          fi
-          echo "::remove-matcher owner=problem-matcher-gcc-ubsan::"
-          echo "::remove-matcher owner=problem-matcher-gcc-asan::"
-          echo "::remove-matcher owner=problem-matcher-gcc-tsan::"
-
-      - name: Show statistics for ccache
-        uses: ./.github/actions/log-ccache-stats
-
-  valgrind:
-    name: Valgrind memcheck
-    needs: [ workspace-container, test ]
-    runs-on: [ self-hosted, vwire ]
-    container:
-      image: ${{ needs.workspace-container.outputs.image }}
-    steps:
-      - name: Fetch the package's repository
-        uses: actions/checkout@v4
-        # NOTE: Even though the repo is already present in the container, we
-        #       also need to check it out at GitHub Actions' preferred location
-        #       for private actions and problem matchers to work.
-
-      - name: Setup ccache
-        uses: ./.github/actions/setup-ccache
-        with:
-          cache-group: noetic-gcc-release
-          cache-version: ${{ secrets.CCACHE_CACHE_VERSION }}
-
-      - name: Install Valgrind
-        run: |
-          apt-get update
-          apt-get install -q -y --no-install-recommends valgrind
-
-      - name: Build regular code
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: catkin build wavemap_all --no-status --force-color
-
-      - name: Build unit tests
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: catkin build wavemap_all --no-status --force-color --no-deps --catkin-make-args tests
-
-      - name: Check unit tests with Valgrind memcheck
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          echo "::add-matcher::./.github/problem-matchers/valgrind.json"
-          all_tests_passed=1
-          source devel/setup.bash
-          for f in devel/lib/wavemap*/test_*
-            do valgrind --tool=memcheck --leak-check=full --leak-resolution=high --num-callers=20 --track-origins=yes --show-possibly-lost=no --errors-for-leak-kinds=definite,indirect --error-exitcode=1 --xml=yes --xml-file=valgrind-log.xml $f --gtest_color=yes || all_tests_passed=0
-            grep -Poz '(?<=<error>)(.*\n)*.*(?=</error>)' valgrind-log.xml || true
-          done
-          if [ $all_tests_passed -ne 1 ]; then
-            echo "Not all tests passed!"
-            exit 1
-          fi
-          echo "::remove-matcher owner=problem-matcher-valgrind::"
-
-      - name: Show statistics for ccache
-        uses: ./.github/actions/log-ccache-stats
diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
new file mode 100644
index 000000000..9876de5cf
--- /dev/null
+++ b/.github/workflows/lint.yml
@@ -0,0 +1,53 @@
+name: C++ Library
+
+on:
+  pull_request:
+    branches: [ main ]
+
+env:
+  PRE_COMMIT_DIR: /home/ci/pre-commit
+
+jobs:
+  pre-commit:
+    name: Pre-commit
+    runs-on: ubuntu-20.04
+    container:
+      # NOTE: Pylint checks if all modules that are marked for import are
+      #       available. At the time of writing, the python scripts in this repo
+      #       only depend on modules that are present on noetic-ros-base-focal
+      #       out of the box. If scripts are added later that depend on custom
+      #       package (e.g. installed through rosdep or pulled in through
+      #       vcstool), it'd make sense to run pre-commit in a full workspace
+      #       container (such as ${{ needs.workspace-container.outputs.image }})
+      #       at the cost of a longer loading time on the CI actions runner.
+      image: ros:noetic-ros-base-focal
+    steps:
+      - name: Install pre-commit's dependencies
+        run: |
+          apt-get update
+          apt-get install -q -y --no-install-recommends git python3-pip clang-format-11 cppcheck libxml2-utils wget
+          pip3 install pre-commit cpplint
+          wget -O /bin/hadolint https://github.com/hadolint/hadolint/releases/download/v2.8.0/hadolint-Linux-x86_64
+          chmod +x /bin/hadolint
+
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+        # NOTE: This has to be done after installing pre-commit, s.t. the
+        #       pre-commit hooks are automatically initialized.
+
+      - name: Get python version for pre-commit cache
+        run: echo "PRE_COMMIT_PYTHON_VERSION=$(python -VV | sha256sum | cut -d' ' -f1)" >> $GITHUB_ENV
+
+      - name: Setup pre-commit cache sharing
+        uses: actions/cache@v4
+        with:
+          path: ${{ env.PRE_COMMIT_DIR }}
+          key: pre-commit|${{ env.PRE_COMMIT_PYTHON_VERSION }}|${{ hashFiles('.pre-commit-config.yaml') }}
+
+      - name: Run the pre-commit hooks
+        shell: bash
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/pre-commit.json"
+          source /opt/ros/noetic/setup.bash
+          PRE_COMMIT_HOME=${{ env.PRE_COMMIT_DIR }} SKIP=no-commit-to-branch pre-commit run --all-files
+          echo "::remove-matcher owner=problem-matcher-pre-commit::"

From 5d5d10b9272bce880272bf524978f7895592d1b7 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 4 Sep 2024 14:40:44 +0200
Subject: [PATCH 53/85] Run pre-commit directly on VM not in Docker

---
 .github/workflows/lint.yml | 28 +++++++---------------------
 1 file changed, 7 insertions(+), 21 deletions(-)

diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml
index 9876de5cf..f195159a6 100644
--- a/.github/workflows/lint.yml
+++ b/.github/workflows/lint.yml
@@ -1,34 +1,21 @@
-name: C++ Library
+name: Lint
 
 on:
   pull_request:
     branches: [ main ]
 
-env:
-  PRE_COMMIT_DIR: /home/ci/pre-commit
-
 jobs:
   pre-commit:
     name: Pre-commit
     runs-on: ubuntu-20.04
-    container:
-      # NOTE: Pylint checks if all modules that are marked for import are
-      #       available. At the time of writing, the python scripts in this repo
-      #       only depend on modules that are present on noetic-ros-base-focal
-      #       out of the box. If scripts are added later that depend on custom
-      #       package (e.g. installed through rosdep or pulled in through
-      #       vcstool), it'd make sense to run pre-commit in a full workspace
-      #       container (such as ${{ needs.workspace-container.outputs.image }})
-      #       at the cost of a longer loading time on the CI actions runner.
-      image: ros:noetic-ros-base-focal
     steps:
       - name: Install pre-commit's dependencies
         run: |
-          apt-get update
-          apt-get install -q -y --no-install-recommends git python3-pip clang-format-11 cppcheck libxml2-utils wget
+          sudo apt-get update
+          sudo apt-get install -q -y --no-install-recommends git python3-pip clang-format-11 cppcheck libxml2-utils wget
           pip3 install pre-commit cpplint
-          wget -O /bin/hadolint https://github.com/hadolint/hadolint/releases/download/v2.8.0/hadolint-Linux-x86_64
-          chmod +x /bin/hadolint
+          sudo wget -O /bin/hadolint https://github.com/hadolint/hadolint/releases/download/v2.8.0/hadolint-Linux-x86_64
+          sudo chmod +x /bin/hadolint
 
       - name: Fetch the package's repository
         uses: actions/checkout@v4
@@ -41,13 +28,12 @@ jobs:
       - name: Setup pre-commit cache sharing
         uses: actions/cache@v4
         with:
-          path: ${{ env.PRE_COMMIT_DIR }}
+          path: ~/.cache/pre-commit
           key: pre-commit|${{ env.PRE_COMMIT_PYTHON_VERSION }}|${{ hashFiles('.pre-commit-config.yaml') }}
 
       - name: Run the pre-commit hooks
         shell: bash
         run: |
           echo "::add-matcher::./.github/problem-matchers/pre-commit.json"
-          source /opt/ros/noetic/setup.bash
-          PRE_COMMIT_HOME=${{ env.PRE_COMMIT_DIR }} SKIP=no-commit-to-branch pre-commit run --all-files
+          PRE_COMMIT_HOME=~/.cache/pre-commit SKIP=no-commit-to-branch pre-commit run --all-files
           echo "::remove-matcher owner=problem-matcher-pre-commit::"

From 1f376e52908fb46254753361fe8a7fdd7788a2b8 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 4 Sep 2024 15:03:18 +0200
Subject: [PATCH 54/85] Add CI workflow for the C++ API

---
 .github/actions/log-ccache-stats/action.yml | 16 ---------
 .github/actions/setup-ccache/action.yml     | 40 ---------------------
 .github/workflows/cpp.yml                   | 35 ++++++++++++++++++
 3 files changed, 35 insertions(+), 56 deletions(-)
 delete mode 100644 .github/actions/log-ccache-stats/action.yml
 delete mode 100644 .github/actions/setup-ccache/action.yml
 create mode 100644 .github/workflows/cpp.yml

diff --git a/.github/actions/log-ccache-stats/action.yml b/.github/actions/log-ccache-stats/action.yml
deleted file mode 100644
index f400c62a3..000000000
--- a/.github/actions/log-ccache-stats/action.yml
+++ /dev/null
@@ -1,16 +0,0 @@
-name: 'Log ccache stats'
-description: 'Log statistics for ccache if it was enabled'
-
-runs:
-  using: "composite"
-  steps:
-    - name: Ccache statistics
-      shell: bash
-      run: |
-        if [ "$(which gcc)" == "/usr/lib/ccache/gcc" ]; then
-          echo "Using ccache: true"
-          echo "Ccache stats"
-          ccache --show-stats
-        else
-          echo "Using ccache: FALSE"
-        fi
diff --git a/.github/actions/setup-ccache/action.yml b/.github/actions/setup-ccache/action.yml
deleted file mode 100644
index 1234c1222..000000000
--- a/.github/actions/setup-ccache/action.yml
+++ /dev/null
@@ -1,40 +0,0 @@
-name: 'Setup ccache'
-description: 'Install ccache and configure it to use GitHub cache sharing'
-inputs:
-  cache-group:
-    description: 'Key used to separate ccache caches on GitHub from different configurations'
-    required: true
-  cache-version:
-    description: 'Key used to manually flush the cache (e.g. by setting a GitHub secret to a new random hash)'
-    required: true
-
-runs:
-  using: "composite"
-  steps:
-    - name: Get the current date (used for cache matching)
-      id: get-date
-      shell: bash
-      run: echo "date=$(date -u "+%Y-%m-%d_%H-%M-%S")" >> $GITHUB_OUTPUT
-
-    - name: Setup ccache cache sharing
-      uses: actions/cache@v4
-      with:
-        path: ${{ env.CCACHE_DIR }}
-        key: ccache-${{ inputs.cache-version }}-${{ inputs.cache-group }}-${{ github.sha }}-${{ steps.get-date.outputs.date }}
-        restore-keys: |
-          ccache-${{ inputs.cache-version }}-${{ inputs.cache-group }}-${{ github.sha }}-
-          ccache-${{ inputs.cache-version }}-${{ inputs.cache-group }}-
-        # NOTE: The action internally also gives priority to caches that were
-        #       created for the same git branch, i.e. it first tries to match
-        #       the restore keys against the current branch and then main.
-
-    - name: Configure ccache
-      shell: bash
-      run: |
-        echo "PATH="/usr/lib/ccache:$PATH"" >> $GITHUB_ENV
-        ccache --max-size=1G
-        ccache --set-config=compiler_check=content
-
-    - name: Reset ccache stats to get per-run statistics
-      shell: bash
-      run: ccache --zero-stats
diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
new file mode 100644
index 000000000..6651e566a
--- /dev/null
+++ b/.github/workflows/cpp.yml
@@ -0,0 +1,35 @@
+name: C++ API
+
+on:
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  build:
+    name: Build
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Setup CMake
+        uses: jwlawson/actions-setup-cmake@v2
+        with:
+          cmake-version: '3.18'
+
+      - name: Setup ccache
+        uses: hendrikmuhs/ccache-action@v1.2
+        with:
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-release
+          create-symlink: true
+
+      - name: Configure CMake
+        working-directory: ${{github.workspace}}
+        run: cmake -B build -DCMAKE_BUILD_TYPE=Release library/cpp
+
+      - name: Build
+        working-directory: ${{github.workspace}}
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/gcc.json"
+          cmake --build build --config Release
+          echo "::remove-matcher owner=problem-matcher-gcc::"

From f7b67712ca84ba2240f0fef7620316fac8551c36 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 4 Sep 2024 16:09:37 +0200
Subject: [PATCH 55/85] Run C++ API tests in CI

---
 .github/workflows/cpp.yml | 49 ++++++++++++++++++++++++++++++++++++++-
 1 file changed, 48 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index 6651e566a..0edd001bf 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -31,5 +31,52 @@ jobs:
         working-directory: ${{github.workspace}}
         run: |
           echo "::add-matcher::./.github/problem-matchers/gcc.json"
-          cmake --build build --config Release
+          cmake --build build --parallel --config Release
           echo "::remove-matcher owner=problem-matcher-gcc::"
+
+  test:
+    name: Test
+    needs: build
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Setup CMake
+        uses: jwlawson/actions-setup-cmake@v2
+        with:
+          cmake-version: '3.18'
+
+      - name: Setup ccache
+        uses: hendrikmuhs/ccache-action@v1.2
+        with:
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-release
+          create-symlink: true
+
+      - name: Setup GTest
+        run: |
+          sudo apt-get update
+          sudo apt-get install -q -y --no-install-recommends libgtest-dev
+
+      - name: Configure CMake
+        working-directory: ${{github.workspace}}
+        run: cmake -B build -DCMAKE_BUILD_TYPE=Release -DENABLE_TESTING=ON library/cpp
+
+      - name: Build tests
+        working-directory: ${{github.workspace}}
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/gcc.json"
+          cmake --build build --parallel --config Release
+          echo "::remove-matcher owner=problem-matcher-gcc::"
+
+      - name: Run tests
+        working-directory: ${{github.workspace}}
+        run: |
+          all_tests_passed=1
+          for f in `find build/test/src/*/test_* -executable`; do
+            $f --gtest_color=yes || all_tests_passed=0
+          done
+          if [ $all_tests_passed -ne 1 ]; then
+            echo "Not all tests passed!"
+            exit 1
+          fi

From 82f90886f72cdf38f1cf37932ee23526fe33a4c3 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 4 Sep 2024 16:42:52 +0200
Subject: [PATCH 56/85] Add CI workflow for the Python API

---
 .github/workflows/cpp.yml     |  2 +-
 .github/workflows/python.yml  | 27 +++++++++++++++++++++++++++
 library/python/CMakeLists.txt |  4 ++--
 library/python/pyproject.toml |  2 +-
 4 files changed, 31 insertions(+), 4 deletions(-)
 create mode 100644 .github/workflows/python.yml

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index 0edd001bf..cf6e26e23 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -56,7 +56,7 @@ jobs:
       - name: Setup GTest
         run: |
           sudo apt-get update
-          sudo apt-get install -q -y --no-install-recommends libgtest-dev
+          sudo apt-get install -yq --no-install-recommends libgtest-dev
 
       - name: Configure CMake
         working-directory: ${{github.workspace}}
diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
new file mode 100644
index 000000000..d9533970a
--- /dev/null
+++ b/.github/workflows/python.yml
@@ -0,0 +1,27 @@
+name: Python API
+
+on:
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  build:
+    name: Build and Test
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Setup Python
+        uses: actions/setup-python@v3
+
+      - name: Upgrade pip
+        run: python -m pip install --upgrade pip
+
+      - name: Build
+        run: python -m pip install -v ./library/python/
+
+      - name: Test
+        run: |
+          python -m pip install -v './library/python[test]'
+          pytest -rAv ./library/python/
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 17d8d74fa..1927b1179 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -27,9 +27,9 @@ endif ()
 
 # Load the wavemap library (if not already loaded)
 if (NOT TARGET wavemap::wavemap_core)
-  if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../../library/cpp)
+  if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/)
     message(STATUS "Loading wavemap library installed on system")
-    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../library/cpp
+    add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp
         ${CMAKE_CURRENT_BINARY_DIR}/wavemap)
   else ()
     set(WAVEMAP_VERSION main)
diff --git a/library/python/pyproject.toml b/library/python/pyproject.toml
index 2139aaa15..7d98f6b5d 100644
--- a/library/python/pyproject.toml
+++ b/library/python/pyproject.toml
@@ -17,7 +17,7 @@ classifiers = ["License :: BSD3"]
 Homepage = "https://github.com/ethz-asl/wavemap"
 
 [project.optional-dependencies]
-test = ["pytest"]
+test = ["pytest", "numpy"]
 
 [tool.scikit-build]
 build-dir = "build/{wheel_tag}"

From bd42e7b43cf1e0d038ff0aa5b45750336a545a39 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Wed, 4 Sep 2024 21:12:34 +0200
Subject: [PATCH 57/85] Address Python API CI build errors

---
 library/cpp/CMakeLists.txt    |  2 ++
 library/python/CMakeLists.txt | 24 +++++++++---------------
 2 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt
index c854e93a1..e917d9efe 100644
--- a/library/cpp/CMakeLists.txt
+++ b/library/cpp/CMakeLists.txt
@@ -6,6 +6,8 @@ cmake_policy(SET CMP0077 NEW)
 cmake_policy(SET CMP0079 NEW)
 option(GENERATE_WAVEMAP_INSTALL_RULES
     "Whether to generate install rules for the wavemap library" ON)
+option(BUILD_SHARED_LIBS
+    "Whether to build wavemap as a shared library" ON)
 option(USE_SYSTEM_EIGEN "Use system pre-installed Eigen" ON)
 option(USE_SYSTEM_GLOG "Use system pre-installed glog" ON)
 option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON)
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 1927b1179..38a7ab870 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -28,22 +28,16 @@ endif ()
 # Load the wavemap library (if not already loaded)
 if (NOT TARGET wavemap::wavemap_core)
   if (EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/../cpp/)
-    message(STATUS "Loading wavemap library installed on system")
+    message(STATUS "Loading wavemap C++ library sources")
+    set(GENERATE_WAVEMAP_INSTALL_RULES OFF)
+    set(BUILD_SHARED_LIBS OFF)
     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../cpp
         ${CMAKE_CURRENT_BINARY_DIR}/wavemap)
   else ()
-    set(WAVEMAP_VERSION main)
-    message(STATUS
-        "Loading wavemap library from GitHub (ref ${WAVEMAP_VERSION})")
-    cmake_minimum_required(VERSION 3.18)
-    include(FetchContent)
-    FetchContent_Declare(
-        ext_wavemap PREFIX wavemap
-        GIT_REPOSITORY https://github.com/ethz-asl/wavemap.git
-        GIT_TAG ${WAVEMAP_VERSION}
-        GIT_SHALLOW 1
-        SOURCE_SUBDIR library/cpp)
-    FetchContent_MakeAvailable(ext_wavemap)
+    message(ERROR
+        "Can not load wavemap C++ library sources. Are you using an old "
+        "version of pip? If so, retry after upgrading with "
+        "\"python3 -m pip install --upgrade pip\".")
   endif ()
 endif ()
 
@@ -71,8 +65,8 @@ nanobind_add_module(_pywavemap_bindings STABLE_ABI
     src/param.cc
     src/pipeline.cc)
 set_wavemap_target_properties(_pywavemap_bindings)
-target_include_directories(_pywavemap_bindings PUBLIC include)
-target_link_libraries(_pywavemap_bindings PUBLIC
+target_include_directories(_pywavemap_bindings PRIVATE include)
+target_link_libraries(_pywavemap_bindings PRIVATE
     wavemap::wavemap_core wavemap::wavemap_io wavemap::wavemap_pipeline)
 # Disable some default wavemap warnings that trigger on nanobind
 target_compile_options(_pywavemap_bindings PRIVATE

From a4eba444e5030d8e268463fe18c4d00516b310a2 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 15:56:20 +0200
Subject: [PATCH 58/85] Automatically download necessary data for testing

---
 .github/workflows/python.yml          | 17 +++++++++++++++--
 library/python/test/conftest.py       | 18 ++++++++++++++++++
 library/python/test/data/.gitignore   |  3 +++
 library/python/test/test_pywavemap.py |  7 +++++--
 4 files changed, 41 insertions(+), 4 deletions(-)
 create mode 100644 library/python/test/conftest.py
 create mode 100644 library/python/test/data/.gitignore

diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index d9533970a..c01263513 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -6,14 +6,14 @@ on:
 
 jobs:
   build:
-    name: Build and Test
+    name: Build
     runs-on: ubuntu-20.04
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
 
       - name: Setup Python
-        uses: actions/setup-python@v3
+        uses: actions/setup-python@v5
 
       - name: Upgrade pip
         run: python -m pip install --upgrade pip
@@ -21,6 +21,19 @@ jobs:
       - name: Build
         run: python -m pip install -v ./library/python/
 
+  test:
+    name: Test
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Setup Python
+        uses: actions/setup-python@v5
+
+      - name: Upgrade pip
+        run: python -m pip install --upgrade pip
+
       - name: Test
         run: |
           python -m pip install -v './library/python[test]'
diff --git a/library/python/test/conftest.py b/library/python/test/conftest.py
new file mode 100644
index 000000000..6d8f81708
--- /dev/null
+++ b/library/python/test/conftest.py
@@ -0,0 +1,18 @@
+from os import path
+from urllib.request import urlretrieve
+
+
+def pytest_sessionstart():
+    """
+    Called after the Session object has been created and
+    before performing collection and entering the run test loop.
+    """
+
+    # Make sure a dummy map is available for testing
+    test_data_dir = path.join(path.dirname(path.abspath(__file__)), "data")
+    map_url = "https://drive.google.com/uc?export=download&id=1OAgswwdJD11Ahq4x3NHQ-YElXQfkGRk3"
+    map_name = "dummy_map.wvmp"
+    map_storage_path = path.join(test_data_dir, map_name)
+    if not path.exists(map_storage_path):
+        print("Downloading dummy map for testing")
+        urlretrieve(map_url, map_storage_path)
diff --git a/library/python/test/data/.gitignore b/library/python/test/data/.gitignore
new file mode 100644
index 000000000..28dad2e17
--- /dev/null
+++ b/library/python/test/data/.gitignore
@@ -0,0 +1,3 @@
+# Ignore everything in this directory except the .gitignore file itself
+*
+!.gitignore
diff --git a/library/python/test/test_pywavemap.py b/library/python/test/test_pywavemap.py
index 185cfdd5b..d6e689712 100644
--- a/library/python/test/test_pywavemap.py
+++ b/library/python/test/test_pywavemap.py
@@ -1,8 +1,11 @@
 # pylint: disable=import-outside-toplevel
 def load_test_map():
+    from os.path import dirname, abspath, join
     import pywavemap
-    return pywavemap.Map.load(
-        "/home/victor/data/wavemaps/newer_college_mine_10cm.wvmp")
+    test_data_dir = join(dirname(abspath(__file__)), "data")
+    map_name = "dummy_map.wvmp"
+    map_path = join(test_data_dir, map_name)
+    return pywavemap.Map.load(map_path)
 
 
 def test_import():

From 525f48c74b91853ca9b1ddeea72dcf66d72bc94c Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 16:01:21 +0200
Subject: [PATCH 59/85] Add CI workflow for documentation generation

---
 .github/workflows/docs.yml        | 71 +++++++++++++++++++++++++++++++
 .github/workflows/python.yml      |  1 +
 docs/pages/installation/cmake.rst |  4 +-
 3 files changed, 74 insertions(+), 2 deletions(-)
 create mode 100644 .github/workflows/docs.yml

diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml
new file mode 100644
index 000000000..d96d549a0
--- /dev/null
+++ b/.github/workflows/docs.yml
@@ -0,0 +1,71 @@
+name: Documentation
+
+on:
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  build-docs:
+    name: Build docs
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Install dependencies (doxygen+sphinx+breathe+exhale toolchain)
+        run: |
+          sudo apt-get update
+          sudo apt-get install -q -y --no-install-recommends python3-pip doxygen
+          sudo apt-get install -q -y --no-install-recommends latexmk texlive-latex-extra tex-gyre texlive-fonts-recommended texlive-latex-recommended
+          python3 -m pip install --upgrade pip
+          pip3 install exhale sphinx-sitemap sphinx-design sphinx-notfound-page
+          pip3 install sphinxawesome-theme --pre
+          pip3 install "sphinx<7,>6"
+
+      - name: Parse C++ API with Doxygen
+        working-directory: ${{github.workspace}}/docs
+        shell: bash
+        run: doxygen Doxyfile_cpp
+
+      - name: Parse ROS1 Interface with Doxygen
+        working-directory: ${{github.workspace}}/docs
+        shell: bash
+        run: doxygen Doxyfile_ros1
+
+      - name: Build Python API (parsed by Sphinx)
+        run: python -m pip install -v ./library/python/
+
+      - name: Build documentation site
+        working-directory: ${{github.workspace}}/docs
+        shell: bash
+        run: sphinx-build -b html . _build/html
+
+      - name: Bundle site sources into tarball
+        shell: bash
+        run: |
+          tar \
+            --dereference --hard-dereference \
+            --directory ${{github.workspace}}/docs/_build/html/ \
+            -cvf ${{github.workspace}}/docs/artifact.tar \
+            --exclude=.git \
+            --exclude=.github \
+            .
+
+      - name: Upload tarball as GH Pages artifact
+        uses: actions/upload-artifact@main
+        with:
+          name: github-pages
+          path: ${{github.workspace}}/docs/artifact.tar
+          retention-days: 1
+
+      - name: Build documentation PDF
+        working-directory: ${{github.workspace}}/docs
+        shell: bash
+        run: sphinx-build -M latexpdf . _build/latex
+
+      - name: Upload PDF
+        uses: actions/upload-artifact@main
+        with:
+          name: documentation-pdf
+          path: ${{github.workspace}}/docs/_build/latex/latex/wavemap.pdf
+          retention-days: 3
diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index c01263513..98ddd77b4 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -23,6 +23,7 @@ jobs:
 
   test:
     name: Test
+    needs: build
     runs-on: ubuntu-20.04
     steps:
       - name: Fetch the package's repository
diff --git a/docs/pages/installation/cmake.rst b/docs/pages/installation/cmake.rst
index 8a6119c05..0816a51c3 100644
--- a/docs/pages/installation/cmake.rst
+++ b/docs/pages/installation/cmake.rst
@@ -1,5 +1,5 @@
-C++ Library (CMake)
-###################
+C++ (CMake)
+###########
 .. highlight:: bash
 .. rstcheck: ignore-directives=tab-set-code
 .. rstcheck: ignore-roles=gh_file

From bdf2e8ae5b9b14b881e181c722997c5a4552a5bd Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 16:33:33 +0200
Subject: [PATCH 60/85] Add clang-tidy to C++ API CI workflow

---
 .github/workflows/cpp.yml                     | 29 +++++++++++++++++++
 interfaces/ros1/wavemap_ros/CMakeLists.txt    |  3 --
 .../wavemap_ros_conversions/CMakeLists.txt    |  3 --
 library/cpp/CMakeLists.txt                    |  4 ++-
 library/cpp/cmake/wavemap-extras.cmake        |  8 -----
 5 files changed, 32 insertions(+), 15 deletions(-)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index cf6e26e23..871b22c13 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -80,3 +80,32 @@ jobs:
             echo "Not all tests passed!"
             exit 1
           fi
+
+  clang-tidy:
+    name: Clang tidy
+    needs: [ build ]
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Setup CMake
+        uses: jwlawson/actions-setup-cmake@v2
+        with:
+          cmake-version: '3.18'
+
+      - name: Setup clang-tidy
+        run: |
+          sudo apt-get update
+          sudo apt-get install -q -y --no-install-recommends clang-tidy
+
+      - name: Configure CMake
+        working-directory: ${{github.workspace}}
+        run: cmake -B build -DCMAKE_BUILD_TYPE=Release library/cpp
+
+      - name: Run clang-tidy
+        working-directory: ${{github.workspace}}/build
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/clang-tidy.json"
+          run-clang-tidy -header-filter="*include/wavemap/*" -quiet
+          echo "::remove-matcher owner=problem-matcher-clang-tidy::"
diff --git a/interfaces/ros1/wavemap_ros/CMakeLists.txt b/interfaces/ros1/wavemap_ros/CMakeLists.txt
index 34d6a9568..e90bc7449 100644
--- a/interfaces/ros1/wavemap_ros/CMakeLists.txt
+++ b/interfaces/ros1/wavemap_ros/CMakeLists.txt
@@ -27,9 +27,6 @@ if (livox_ros_driver2_FOUND)
   add_compile_definitions(LIVOX_AVAILABLE)
 endif ()
 
-# Enable general wavemap tooling (e.g. to run clang-tidy CI)
-enable_wavemap_general_tooling()
-
 # Libraries
 add_library(${PROJECT_NAME}
     src/inputs/depth_image_topic_input.cc
diff --git a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt
index 6a2e2f381..2c505017e 100644
--- a/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt
+++ b/interfaces/ros1/wavemap_ros_conversions/CMakeLists.txt
@@ -11,9 +11,6 @@ catkin_package(
     LIBRARIES ${PROJECT_NAME}
     CATKIN_DEPENDS roscpp eigen_conversions wavemap wavemap_msgs)
 
-# Enable general wavemap tooling (e.g. to run clang-tidy CI)
-enable_wavemap_general_tooling()
-
 # Libraries
 add_library(${PROJECT_NAME}
     src/config_conversions.cc
diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt
index e917d9efe..b4a50fba4 100644
--- a/library/cpp/CMakeLists.txt
+++ b/library/cpp/CMakeLists.txt
@@ -15,7 +15,9 @@ option(USE_SYSTEM_BOOST "Use system pre-installed Boost" ON)
 # CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI)
 include(GNUInstallDirs)
 include(cmake/wavemap-extras.cmake)
-enable_wavemap_general_tooling()
+
+# Export compilation database for compatibility with clang-tidy
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 
 # Dependencies
 include(cmake/find-wavemap-deps.cmake)
diff --git a/library/cpp/cmake/wavemap-extras.cmake b/library/cpp/cmake/wavemap-extras.cmake
index ecd761aa7..aa7a2a48b 100644
--- a/library/cpp/cmake/wavemap-extras.cmake
+++ b/library/cpp/cmake/wavemap-extras.cmake
@@ -8,14 +8,6 @@ option(ENABLE_COVERAGE_TESTING
     "Compile with necessary flags for coverage testing" OFF)
 option(USE_CLANG_TIDY "Generate necessary files to run clang-tidy" OFF)
 
-# Enable general wavemap tooling for the calling CMake project
-function(enable_wavemap_general_tooling)
-  # Export compilation database for compatibility with clang-tidy
-  if (USE_CLANG_TIDY)
-    set(CMAKE_EXPORT_COMPILE_COMMANDS ON PARENT_SCOPE)
-  endif ()
-endfunction()
-
 # Adds the include paths of the wavemap library to the given target.
 function(add_wavemap_include_directories target)
   # Configure the include dirs

From eb96ab5994bbfa208ef4ab99307c46948de66662 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 16:43:46 +0200
Subject: [PATCH 61/85] Add Valgrind to C++ API CI workflow

---
 .github/workflows/cpp.yml | 50 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 50 insertions(+)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index 871b22c13..69451b1cd 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -109,3 +109,53 @@ jobs:
           echo "::add-matcher::./.github/problem-matchers/clang-tidy.json"
           run-clang-tidy -header-filter="*include/wavemap/*" -quiet
           echo "::remove-matcher owner=problem-matcher-clang-tidy::"
+
+  valgrind:
+    name: Valgrind memcheck
+    needs: test
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Setup CMake
+        uses: jwlawson/actions-setup-cmake@v2
+        with:
+          cmake-version: '3.18'
+
+      - name: Setup ccache
+        uses: hendrikmuhs/ccache-action@v1.2
+        with:
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-release
+          create-symlink: true
+
+      - name: Setup GTest and Valgrind
+        run: |
+          sudo apt-get update
+          sudo apt-get install -yq --no-install-recommends libgtest-dev valgrind
+
+      - name: Configure CMake
+        working-directory: ${{github.workspace}}
+        run: cmake -B build -DCMAKE_BUILD_TYPE=Release -DENABLE_TESTING=ON library/cpp
+
+      - name: Build tests
+        working-directory: ${{github.workspace}}
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/gcc.json"
+          cmake --build build --parallel --config Release
+          echo "::remove-matcher owner=problem-matcher-gcc::"
+
+      - name: Check unit tests with Valgrind memcheck
+        working-directory: ${{github.workspace}}
+        run: |
+          all_tests_passed=1
+          echo "::add-matcher::./.github/problem-matchers/valgrind.json"
+          for f in `find build/test/src/*/test_* -executable`; do
+            valgrind --tool=memcheck --leak-check=full --leak-resolution=high --num-callers=20 --track-origins=yes --show-possibly-lost=no --errors-for-leak-kinds=definite,indirect --error-exitcode=1 --xml=yes --xml-file=valgrind-log.xml $f --gtest_color=yes || all_tests_passed=0
+            grep -Poz '(?<=<error>)(.*\n)*.*(?=</error>)' valgrind-log.xml || true
+          done
+          echo "::remove-matcher owner=problem-matcher-valgrind::"
+          if [ $all_tests_passed -ne 1 ]; then
+            echo "Not all tests passed!"
+            exit 1
+          fi

From 94c2f98df6d85d6a16a3ff08e687921f14e63fdb Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 17:08:21 +0200
Subject: [PATCH 62/85] Fix clang-tidy treating dependencies are project code

---
 .github/workflows/cpp.yml | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index 69451b1cd..80b88a0ad 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -94,10 +94,12 @@ jobs:
         with:
           cmake-version: '3.18'
 
-      - name: Setup clang-tidy
+      - name: Setup clang-tidy and system deps
         run: |
           sudo apt-get update
           sudo apt-get install -q -y --no-install-recommends clang-tidy
+          # NOTE: The following deps are installed s.t. clang-tidy correctly treats them as system deps
+          sudo apt-get install -q -y --no-install-recommends libeigen3-dev libgoogle-glog-dev libboost-dev
 
       - name: Configure CMake
         working-directory: ${{github.workspace}}
@@ -107,7 +109,7 @@ jobs:
         working-directory: ${{github.workspace}}/build
         run: |
           echo "::add-matcher::./.github/problem-matchers/clang-tidy.json"
-          run-clang-tidy -header-filter="*include/wavemap/*" -quiet
+          run-clang-tidy -quiet -header-filter="*include/wavemap/*"
           echo "::remove-matcher owner=problem-matcher-clang-tidy::"
 
   valgrind:

From cd271429a1cd640f306b64dd45375917ac322d8f Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 17:19:35 +0200
Subject: [PATCH 63/85] Add GCC's sanitizers to C++ API workflow (dynamic
 analysis)

---
 .github/workflows/cpp.yml | 68 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 68 insertions(+)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index 80b88a0ad..b99fff330 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -161,3 +161,71 @@ jobs:
             echo "Not all tests passed!"
             exit 1
           fi
+
+  sanitize:
+    name: Sanitize ${{ matrix.sanitizer.detects }}
+    needs: test
+    runs-on: ubuntu-20.04
+    strategy:
+      matrix:
+        sanitizer:
+          - { name: UBSAN, detects: 'undefined behavior' }
+          - { name: ASAN, detects: 'addressability and leaks' }
+          # - { name: TSAN, detects: 'data races and deadlocks' }
+          # NOTE: TSAN is disabled until the following bug is resolved:
+          #       https://bugs.launchpad.net/ubuntu/+source/gcc-10/+bug/2029910.
+          # NOTE: MSAN is not used for now since it also requires all deps to be
+          #       instrumented (recompiled with clang and the MSan flags, LLVM's
+          #       stdlib instead of GCCs,...). We therefore use Valgrind to
+          #       check for uninitialized memory usage errors instead.
+      fail-fast: false
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+
+      - name: Setup CMake
+        uses: jwlawson/actions-setup-cmake@v2
+        with:
+          cmake-version: '3.18'
+
+      - name: Setup ccache
+        uses: hendrikmuhs/ccache-action@v1.2
+        with:
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-${{ matrix.sanitizer.name }}
+          create-symlink: true
+
+      - name: Setup GTest
+        run: |
+          sudo apt-get update
+          sudo apt-get install -yq --no-install-recommends libgtest-dev
+
+      - name: Configure CMake
+        working-directory: ${{github.workspace}}
+        run: cmake -B build -DCMAKE_BUILD_TYPE=Release -DENABLE_TESTING=ON -DUSE_${{ matrix.sanitizer.name }}=ON library/cpp
+
+      - name: Build tests
+        working-directory: ${{github.workspace}}
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/gcc.json"
+          cmake --build build --parallel --config Release
+          echo "::remove-matcher owner=problem-matcher-gcc::"
+
+      - name: Check unit tests with ${{ matrix.sanitizer.name }}
+        working-directory: ${{github.workspace}}
+        env:
+          UBSAN_OPTIONS: halt_on_error=1:print_stacktrace=1
+          ASAN_OPTIONS: halt_on_error=1:detect_leaks=1:detect_stack_use_after_return=1
+          TSAN_OPTIONS: halt_on_error=1:second_deadlock_stack=1
+        run: |
+          all_tests_passed=1
+          echo "::add-matcher::./.github/problem-matchers/gcc-sanitizers.json"
+          for f in `find build/test/src/*/test_* -executable`; do
+            $f --gtest_color=yes || all_tests_passed=0
+          done
+          if [ $all_tests_passed -ne 1 ]; then
+            echo "Not all tests passed!"
+            exit 1
+          fi
+          echo "::remove-matcher owner=problem-matcher-gcc-ubsan::"
+          echo "::remove-matcher owner=problem-matcher-gcc-asan::"
+          echo "::remove-matcher owner=problem-matcher-gcc-tsan::"

From fd96924b360a9083467dba2ecf32512c7ed2f705 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 17:53:26 +0200
Subject: [PATCH 64/85] Suppress CI error due to GCC bug

---
 examples/cpp/queries/CMakeLists.txt    | 1 -
 library/cpp/cmake/wavemap-extras.cmake | 3 +--
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/examples/cpp/queries/CMakeLists.txt b/examples/cpp/queries/CMakeLists.txt
index 1aaea9444..4feb8a7d7 100644
--- a/examples/cpp/queries/CMakeLists.txt
+++ b/examples/cpp/queries/CMakeLists.txt
@@ -23,4 +23,3 @@ target_link_libraries(trilinear_interpolation PUBLIC wavemap::wavemap_core)
 add_executable(classification classification.cc)
 set_wavemap_target_properties(classification)
 target_link_libraries(classification PUBLIC wavemap::wavemap_core)
-target_compile_options(classification PRIVATE -Wno-suggest-attribute=const)
diff --git a/library/cpp/cmake/wavemap-extras.cmake b/library/cpp/cmake/wavemap-extras.cmake
index aa7a2a48b..5237acde2 100644
--- a/library/cpp/cmake/wavemap-extras.cmake
+++ b/library/cpp/cmake/wavemap-extras.cmake
@@ -32,8 +32,7 @@ function(set_wavemap_target_properties target)
   set_target_properties(${target} PROPERTIES POSITION_INDEPENDENT_CODE ON)
   target_compile_options(${target} PUBLIC -march=native)
   target_compile_options(${target} PRIVATE
-      -Wall -Wextra -Wpedantic -Wsuggest-attribute=const
-      -Wno-deprecated-copy -Wno-class-memaccess)
+      -Wall -Wextra -Wpedantic -Wno-deprecated-copy -Wno-class-memaccess)
 
   # General C++ defines
   target_compile_definitions(${target} PUBLIC EIGEN_INITIALIZE_MATRICES_BY_NAN)

From 79c6510435f258902d49d6fcb5f0da160734d77a Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Thu, 5 Sep 2024 18:40:22 +0200
Subject: [PATCH 65/85] Add CI workflow for the ROS1 Interface

---
 .github/workflows/ros1.yml | 183 +++++++++++++++++++++++++++++++++++++
 1 file changed, 183 insertions(+)
 create mode 100644 .github/workflows/ros1.yml

diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml
new file mode 100644
index 000000000..3714a3b75
--- /dev/null
+++ b/.github/workflows/ros1.yml
@@ -0,0 +1,183 @@
+name: ROS1 Interface
+
+on:
+  pull_request:
+    branches: [ main ]
+
+env:
+  DOCKER_REGISTRY: ghcr.io
+  DOCKER_CI_IMAGE_NAME: ci_wavemap_ros1
+  USER_HOME: /home/ci
+  CATKIN_WS_PATH: /home/ci/catkin_ws
+  CCACHE_DIR: /home/ci/ccache
+  PRE_COMMIT_DIR: /home/ci/pre-commit
+
+jobs:
+  workspace-container:
+    name: Build ROS1 container
+    runs-on: ubuntu-20.04
+    outputs:
+      image: ${{ steps.ref-names.outputs.ci_image }}
+    steps:
+      - name: Common variables
+        id: ref-names
+        run: |
+          echo "cache=${{ env.DOCKER_REGISTRY }}/ethz-asl/${{ env.DOCKER_CI_IMAGE_NAME }}:buildcache" >> $GITHUB_OUTPUT
+          echo "ci_image=${{ env.DOCKER_REGISTRY }}/ethz-asl/${{ env.DOCKER_CI_IMAGE_NAME }}:${{ github.sha }}" >> $GITHUB_OUTPUT
+
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+        with:
+          path: ${{ github.repository }}
+
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v3
+
+      - name: Log in to ${{ env.DOCKER_REGISTRY }} registry
+        uses: docker/login-action@65b78e6e13532edd9afa3aa52ac7964289d1a9c1
+        with:
+          registry: ${{ env.DOCKER_REGISTRY }}
+          username: ${{ github.actor }}
+          password: ${{ secrets.GITHUB_TOKEN }}
+
+      - name: Build the image
+        uses: docker/build-push-action@v6
+        with:
+          context: ${{ github.repository }}
+          file: ${{ github.repository }}/tooling/docker/ros1/full.Dockerfile
+          target: workspace
+          build-args: |
+            REPOSITORY_NAME=${{ github.repository }}
+            USER_HOME=${{ env.USER_HOME }}
+            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
+            CCACHE_DIR=${{ env.CCACHE_DIR }}
+          load: true
+          cache-from: type=registry,ref=${{ steps.ref-names.outputs.cache }}
+          cache-to: type=registry,mode=max,ref=${{ steps.ref-names.outputs.cache }}
+          tags: ${{ steps.ref-names.outputs.ci_image }}
+
+      - name: Test the image
+        run: docker run --rm ${{ steps.ref-names.outputs.ci_image }}
+
+      - name: Push the image
+        uses: docker/build-push-action@v6
+        with:
+          context: ${{ github.repository }}
+          file: ${{ github.repository }}/tooling/docker/ros1/full.Dockerfile
+          target: workspace
+          build-args: |
+            REPOSITORY_NAME=${{ github.repository }}
+            USER_HOME=${{ env.USER_HOME }}
+            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
+            CCACHE_DIR=${{ env.CCACHE_DIR }}
+          push: true
+          cache-from: type=registry,ref=${{ steps.ref-names.outputs.cache }}
+          tags: ${{ steps.ref-names.outputs.ci_image }}
+
+  build:
+    name: Build
+    needs: workspace-container
+    runs-on: ubuntu-20.04
+    container:
+      image: ${{ needs.workspace-container.outputs.image }}
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+        # NOTE: Even though the repo is already present in the container, we
+        #       also need to check it out at GitHub Actions' preferred location
+        #       for private actions and problem matchers to work.
+
+      - name: Setup ccache
+        uses: hendrikmuhs/ccache-action@v1.2
+        with:
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-ros1
+          create-symlink: true
+
+      - name: Build all wavemap packages
+        working-directory: ${{ env.CATKIN_WS_PATH }}
+        shell: bash
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/gcc.json"
+          catkin build wavemap_all --no-status --force-color
+          echo "::remove-matcher owner=problem-matcher-gcc::"
+
+  test:
+    name: Test
+    needs: [ workspace-container, build ]
+    runs-on: ubuntu-20.04
+    container:
+      image: ${{ needs.workspace-container.outputs.image }}
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+        # NOTE: Even though the repo is already present in the container, we
+        #       also need to check it out at GitHub Actions' preferred location
+        #       for private actions and problem matchers to work.
+
+      - name: Setup ccache
+        uses: hendrikmuhs/ccache-action@v1.2
+        with:
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-ros1
+          create-symlink: true
+
+      - name: Build regular code
+        working-directory: ${{ env.CATKIN_WS_PATH }}
+        shell: bash
+        run: catkin build wavemap_all --no-status --force-color --cmake-args -DDCHECK_ALWAYS_ON=ON
+
+      - name: Build unit tests
+        working-directory: ${{ env.CATKIN_WS_PATH }}
+        shell: bash
+        run: |
+          echo "::add-matcher::./.github/problem-matchers/gcc.json"
+          catkin build wavemap_all --no-status --force-color --no-deps --cmake-args -DDCHECK_ALWAYS_ON=ON --catkin-make-args tests
+          echo "::remove-matcher owner=problem-matcher-gcc::"
+
+      - name: Run unit tests
+        working-directory: ${{ env.CATKIN_WS_PATH }}
+        shell: bash
+        run: |
+          all_tests_passed=1
+          source devel/setup.bash
+          for f in devel/lib/wavemap*/test_*
+            do $f --gtest_color=yes || all_tests_passed=0
+          done
+          if [ $all_tests_passed -ne 1 ]; then
+            echo "Not all tests passed!"
+            exit 1
+          fi
+
+  install:
+    name: Install
+    needs: [ workspace-container, build ]
+    runs-on: ubuntu-20.04
+    container:
+      image: ${{ needs.workspace-container.outputs.image }}
+    steps:
+      - name: Fetch the package's repository
+        uses: actions/checkout@v4
+        # NOTE: Even though the repo is already present in the container, we
+        #       also need to check it out at GitHub Actions' preferred location
+        #       for private actions and problem matchers to work.
+
+      - name: Setup ccache
+        uses: hendrikmuhs/ccache-action@v1.2
+        with:
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-ros1
+          create-symlink: true
+
+      - name: Enable catkin install mode
+        working-directory: ${{ env.CATKIN_WS_PATH }}
+        shell: bash
+        run: |
+          catkin config --install
+          catkin clean -bdi -y
+
+      - name: Build all wavemap packages
+        working-directory: ${{ env.CATKIN_WS_PATH }}
+        shell: bash
+        run: |
+          . /opt/ros/noetic/setup.sh
+          echo "::add-matcher::./.github/problem-matchers/gcc.json"
+          catkin build wavemap_all --no-status --force-color
+          echo "::remove-matcher owner=problem-matcher-gcc::"

From 50708fd58bf5dae35101a9b8db587da68ca68d32 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Fri, 13 Sep 2024 13:27:01 +0200
Subject: [PATCH 66/85] Update README

---
 .github/workflows/docs.yml |  2 +-
 .github/workflows/ros1.yml | 58 +++++++++++++++++++-------------------
 README.md                  | 20 ++++++-------
 3 files changed, 39 insertions(+), 41 deletions(-)

diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml
index d96d549a0..c8e1763e1 100644
--- a/.github/workflows/docs.yml
+++ b/.github/workflows/docs.yml
@@ -6,7 +6,7 @@ on:
 
 jobs:
   build-docs:
-    name: Build docs
+    name: Build
     runs-on: ubuntu-20.04
     steps:
       - name: Fetch the package's repository
diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml
index 3714a3b75..e409c0012 100644
--- a/.github/workflows/ros1.yml
+++ b/.github/workflows/ros1.yml
@@ -101,8 +101,8 @@ jobs:
           catkin build wavemap_all --no-status --force-color
           echo "::remove-matcher owner=problem-matcher-gcc::"
 
-  test:
-    name: Test
+  install:
+    name: Install
     needs: [ workspace-container, build ]
     runs-on: ubuntu-20.04
     container:
@@ -120,35 +120,24 @@ jobs:
           key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-ros1
           create-symlink: true
 
-      - name: Build regular code
+      - name: Enable catkin install mode
         working-directory: ${{ env.CATKIN_WS_PATH }}
         shell: bash
-        run: catkin build wavemap_all --no-status --force-color --cmake-args -DDCHECK_ALWAYS_ON=ON
+        run: |
+          catkin config --install
+          catkin clean -bdi -y
 
-      - name: Build unit tests
+      - name: Build all wavemap packages
         working-directory: ${{ env.CATKIN_WS_PATH }}
         shell: bash
         run: |
+          . /opt/ros/noetic/setup.sh
           echo "::add-matcher::./.github/problem-matchers/gcc.json"
-          catkin build wavemap_all --no-status --force-color --no-deps --cmake-args -DDCHECK_ALWAYS_ON=ON --catkin-make-args tests
+          catkin build wavemap_all --no-status --force-color
           echo "::remove-matcher owner=problem-matcher-gcc::"
 
-      - name: Run unit tests
-        working-directory: ${{ env.CATKIN_WS_PATH }}
-        shell: bash
-        run: |
-          all_tests_passed=1
-          source devel/setup.bash
-          for f in devel/lib/wavemap*/test_*
-            do $f --gtest_color=yes || all_tests_passed=0
-          done
-          if [ $all_tests_passed -ne 1 ]; then
-            echo "Not all tests passed!"
-            exit 1
-          fi
-
-  install:
-    name: Install
+  test:
+    name: Test
     needs: [ workspace-container, build ]
     runs-on: ubuntu-20.04
     container:
@@ -166,18 +155,29 @@ jobs:
           key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-ros1
           create-symlink: true
 
-      - name: Enable catkin install mode
+      - name: Build regular code
         working-directory: ${{ env.CATKIN_WS_PATH }}
         shell: bash
-        run: |
-          catkin config --install
-          catkin clean -bdi -y
+        run: catkin build wavemap_all --no-status --force-color --cmake-args -DDCHECK_ALWAYS_ON=ON
 
-      - name: Build all wavemap packages
+      - name: Build unit tests
         working-directory: ${{ env.CATKIN_WS_PATH }}
         shell: bash
         run: |
-          . /opt/ros/noetic/setup.sh
           echo "::add-matcher::./.github/problem-matchers/gcc.json"
-          catkin build wavemap_all --no-status --force-color
+          catkin build wavemap_all --no-status --force-color --no-deps --cmake-args -DDCHECK_ALWAYS_ON=ON --catkin-make-args tests
           echo "::remove-matcher owner=problem-matcher-gcc::"
+
+      - name: Run unit tests
+        working-directory: ${{ env.CATKIN_WS_PATH }}
+        shell: bash
+        run: |
+          all_tests_passed=1
+          source devel/setup.bash
+          for f in devel/lib/wavemap*/test_*
+            do $f --gtest_color=yes || all_tests_passed=0
+          done
+          if [ $all_tests_passed -ne 1 ]; then
+            echo "Not all tests passed!"
+            exit 1
+          fi
diff --git a/README.md b/README.md
index 107cda7f3..588f47677 100644
--- a/README.md
+++ b/README.md
@@ -1,29 +1,27 @@
 # Wavemap
 <div>
-<a href="https://github.com/ethz-asl/wavemap/actions/workflows/ci.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/ci.yml?label=test&logo=githubactions&logoColor=white" alt="test"/></a>
-<a href="https://github.com/ethz-asl/wavemap/actions/workflows/cd.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/cd.yml?label=deploy&logo=githubactions&logoColor=white" alt="deploy"/></a>
-<a href="https://ethz-asl.github.io/wavemap/"><img src="https://img.shields.io/badge/docs-online-brightgreen?logo=sphinx" alt="docs"/></a>
-<a href="https://github.com/ethz-asl/wavemap/releases"><img src="https://img.shields.io/github/v/tag/ethz-asl/wavemap?label=release&logo=github" alt="release"/></a>
-<a href="https://github.com/ethz-asl/wavemap/blob/main/LICENSE"><img src="https://img.shields.io/badge/license-BSD%203-blue?logo=bsd" alt="license"/></a>
-<a href="https://ethz-asl.github.io/wavemap/pages/contributing.html"><img src="https://img.shields.io/badge/contributions-welcome-brightgreen" alt="contributions welcome"/></a>
+<a href="https://github.com/ethz-asl/wavemap/actions/workflows/cpp.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/cpp.yml?label=C%2b%2b&logo=C%2b%2b&logoColor=white" alt="C++"/></a>
+<a href="https://github.com/ethz-asl/wavemap/actions/workflows/python.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/python.yml?label=Python&logo=python&logoColor=white" alt="Python"/></a>
+<a href="https://github.com/ethz-asl/wavemap/actions/workflows/ros1.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/ros1.yml?label=ROS1&logo=ros&logoColor=white" alt="ROS1"/></a>
+<a href="https://github.com/ethz-asl/wavemap/actions/workflows/docs.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/docs.yml?label=Docs&logo=sphinx&logoColor=white" alt="Docs"/></a>
 </div>
 <div>
-<a href="https://ethz-asl.github.io/wavemap/pages/installation.html"><img src="https://img.shields.io/badge/Intel-0071C5?logo=intel" alt="Intel"/></a>
-<a href="https://ethz-asl.github.io/wavemap/pages/installation.html"><img src="https://img.shields.io/badge/AMD-ED1C24?logo=amd" alt="AMD"/></a>
-<a href="https://ethz-asl.github.io/wavemap/pages/installation.html"><img src="https://img.shields.io/badge/Arm-0091BD?logo=arm&logoColor=white" alt="Arm"/></a>
-<a href="https://github.com/ethz-asl/wavemap/pkgs/container/wavemap"><img src="https://img.shields.io/badge/Docker-2496ED?logo=docker&logoColor=black" alt="docker"/></a>
+<a href="https://github.com/ethz-asl/wavemap/releases"><img src="https://img.shields.io/github/v/tag/ethz-asl/wavemap?label=Release&logo=github" alt="Release"/></a>
+<a href="https://github.com/ethz-asl/wavemap/blob/main/LICENSE"><img src="https://img.shields.io/badge/License-BSD%203-blue?logo=bsd" alt="License"/></a>
 </div>
 
 [![3D reconstruction of Newer College's Cloister](https://github.com/ethz-asl/wavemap/assets/6238939/e432d4ea-440d-4e9d-adf9-af3ae3b09a10)](https://www.youtube.com/live/ftQhK75Ri1E?si=9txTYyJ78wQuhyN-&t=733)
 
 ## Hierarchical, multi-resolution volumetric mapping
-
 Wavemap achieves state-of-the-art memory and computational efficiency by combining Haar wavelet compression and a coarse-to-fine measurement integration scheme. Advanced measurement models allow it to attain exceptionally high recall rates on challenging obstacles like thin objects.
 
 The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single multi-resolution occupancy grid map.
 
+Wavemap provides [C++](https://ethz-asl.github.io/wavemap/pages/tutorials/cpp) and [Python](https://ethz-asl.github.io/wavemap/pages/tutorials/python) APIs and an interface to [ROS1](https://ethz-asl.github.io/wavemap/pages/tutorials/ros1). The code is extensively tested on Intel, AMD and ARM CPUs. Example Docker files [are available](https://github.com/ethz-asl/wavemap/tree/main/tooling/docker) and documented in the [installation instructions](https://ethz-asl.github.io/wavemap/pages/installation/index). We [welcome contributions](https://ethz-asl.github.io/wavemap/pages/contributing).
+
 ⭐ If you find wavemap useful, star it on GitHub to get notified of new releases!
 
+
 ## Documentation
 The framework's documentation is hosted on [GitHub Pages](https://ethz-asl.github.io/wavemap/).
 

From d58ed7e251e0e5c470f5de884382c1c0204be833 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victor.reijgwart@ethz-asl.ch>
Date: Fri, 13 Sep 2024 13:32:02 +0200
Subject: [PATCH 67/85] Adjust README formatting

---
 README.md | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/README.md b/README.md
index 588f47677..30e455b92 100644
--- a/README.md
+++ b/README.md
@@ -1,15 +1,10 @@
 # Wavemap
-<div>
 <a href="https://github.com/ethz-asl/wavemap/actions/workflows/cpp.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/cpp.yml?label=C%2b%2b&logo=C%2b%2b&logoColor=white" alt="C++"/></a>
 <a href="https://github.com/ethz-asl/wavemap/actions/workflows/python.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/python.yml?label=Python&logo=python&logoColor=white" alt="Python"/></a>
 <a href="https://github.com/ethz-asl/wavemap/actions/workflows/ros1.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/ros1.yml?label=ROS1&logo=ros&logoColor=white" alt="ROS1"/></a>
 <a href="https://github.com/ethz-asl/wavemap/actions/workflows/docs.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/docs.yml?label=Docs&logo=sphinx&logoColor=white" alt="Docs"/></a>
-</div>
-<div>
 <a href="https://github.com/ethz-asl/wavemap/releases"><img src="https://img.shields.io/github/v/tag/ethz-asl/wavemap?label=Release&logo=github" alt="Release"/></a>
 <a href="https://github.com/ethz-asl/wavemap/blob/main/LICENSE"><img src="https://img.shields.io/badge/License-BSD%203-blue?logo=bsd" alt="License"/></a>
-</div>
-
 [![3D reconstruction of Newer College's Cloister](https://github.com/ethz-asl/wavemap/assets/6238939/e432d4ea-440d-4e9d-adf9-af3ae3b09a10)](https://www.youtube.com/live/ftQhK75Ri1E?si=9txTYyJ78wQuhyN-&t=733)
 
 ## Hierarchical, multi-resolution volumetric mapping

From c3fabebcbfcc913dd80b1861e1984ac01c5b940d Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Sat, 14 Sep 2024 11:12:25 +0200
Subject: [PATCH 68/85] Add citation file

---
 CITATION.cff | 18 ++++++++++++++++++
 README.md    |  2 +-
 2 files changed, 19 insertions(+), 1 deletion(-)
 create mode 100644 CITATION.cff

diff --git a/CITATION.cff b/CITATION.cff
new file mode 100644
index 000000000..f3f5de185
--- /dev/null
+++ b/CITATION.cff
@@ -0,0 +1,18 @@
+cff-version: 1.2.0
+preferred-citation:
+  title: "Efficient volumetric mapping of multi-scale environments using wavelet-based compression"
+  authors:
+    - family-names: Reijgwart
+      given-names: Victor
+    - family-names: Cadena
+      given-names: Cesar
+    - family-names: Siegwart
+      given-names: Roland
+    - family-names: Ott
+      given-names: Lionel
+  journal: "Robotics: Science and Systems"
+  year: "2023"
+  type: conference-paper
+  doi: "10.15607/RSS.2023.XIX.065"
+  url: https://www.roboticsproceedings.org/rss19/p065.pdf
+  codeurl: https://github.com/ethz-asl/wavemap
diff --git a/README.md b/README.md
index 30e455b92..b5ba152bc 100644
--- a/README.md
+++ b/README.md
@@ -3,7 +3,7 @@
 <a href="https://github.com/ethz-asl/wavemap/actions/workflows/python.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/python.yml?label=Python&logo=python&logoColor=white" alt="Python"/></a>
 <a href="https://github.com/ethz-asl/wavemap/actions/workflows/ros1.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/ros1.yml?label=ROS1&logo=ros&logoColor=white" alt="ROS1"/></a>
 <a href="https://github.com/ethz-asl/wavemap/actions/workflows/docs.yml"><img src="https://img.shields.io/github/actions/workflow/status/ethz-asl/wavemap/docs.yml?label=Docs&logo=sphinx&logoColor=white" alt="Docs"/></a>
-<a href="https://github.com/ethz-asl/wavemap/releases"><img src="https://img.shields.io/github/v/tag/ethz-asl/wavemap?label=Release&logo=github" alt="Release"/></a>
+<a href="https://github.com/ethz-asl/wavemap/releases"><img src="https://img.shields.io/github/v/tag/ethz-asl/wavemap?label=Version&logo=semver" alt="Version"/></a>
 <a href="https://github.com/ethz-asl/wavemap/blob/main/LICENSE"><img src="https://img.shields.io/badge/License-BSD%203-blue?logo=bsd" alt="License"/></a>
 [![3D reconstruction of Newer College's Cloister](https://github.com/ethz-asl/wavemap/assets/6238939/e432d4ea-440d-4e9d-adf9-af3ae3b09a10)](https://www.youtube.com/live/ftQhK75Ri1E?si=9txTYyJ78wQuhyN-&t=733)
 

From 4339643817aa093505a87264c50af801a2012b77 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 13:15:18 +0200
Subject: [PATCH 69/85] Address the first batch of @LionelOtt's review comments
 on PR#69

---
 examples/cpp/queries/classification.cc        |  4 +--
 examples/python/mapping/full_pipeline.py      | 10 +++----
 examples/python/panoptic_mapping.py           | 14 +++++-----
 .../python/queries/accelerated_queries.py     |  8 +++---
 examples/python/queries/classification.py     | 12 ++++-----
 examples/python/queries/fixed_resolution.py   |  2 +-
 examples/python/queries/multi_resolution.py   |  2 +-
 .../queries/nearest_neighbor_interpolation.py |  2 +-
 .../python/queries/trilinear_interpolation.py |  2 +-
 .../wavemap/core/indexing/ndtree_index.h      |  7 +++--
 .../cpp/include/wavemap/core/map/map_base.h   |  8 +++---
 .../cpp/include/wavemap/pipeline/pipeline.h   |  2 +-
 library/cpp/src/pipeline/pipeline.cc          |  2 +-
 library/python/src/indices.cc                 | 13 +++++----
 library/python/src/maps.cc                    | 20 +++++++-------
 library/python/src/measurements.cc            |  2 +-
 library/python/src/pipeline.cc                | 27 ++++++++++---------
 library/python/test/test_pywavemap.py         |  8 +++---
 18 files changed, 76 insertions(+), 69 deletions(-)

diff --git a/examples/cpp/queries/classification.cc b/examples/cpp/queries/classification.cc
index 6bd37e504..24619ea0b 100644
--- a/examples/cpp/queries/classification.cc
+++ b/examples/cpp/queries/classification.cc
@@ -36,7 +36,7 @@ int main(int, char**) {
 
   // Once a threshold has been chosen, you can either classify in log space
   {
-    const bool is_occupied = kOccupancyThresholdLogOdds < occupancy_log_odds;
+    const bool is_occupied = kOccupancyThresholdLogOdds <= occupancy_log_odds;
     const bool is_free = occupancy_log_odds < kOccupancyThresholdLogOdds;
     examples::doSomething(is_occupied);
     examples::doSomething(is_free);
@@ -44,7 +44,7 @@ int main(int, char**) {
 
   // Or in probability space
   {
-    const bool is_occupied = kOccupancyThresholdProb < occupancy_probability;
+    const bool is_occupied = kOccupancyThresholdProb <= occupancy_probability;
     const bool is_free = occupancy_probability < kOccupancyThresholdProb;
     examples::doSomething(is_occupied);
     examples::doSomething(is_free);
diff --git a/examples/python/mapping/full_pipeline.py b/examples/python/mapping/full_pipeline.py
index 788fc0edf..0df496cc8 100644
--- a/examples/python/mapping/full_pipeline.py
+++ b/examples/python/mapping/full_pipeline.py
@@ -21,14 +21,14 @@
 # Create a measurement integration pipeline
 pipeline = wave.Pipeline(your_map)
 # Add map operations
-pipeline.addOperation({
+pipeline.add_operation({
     "type": "threshold_map",
     "once_every": {
         "seconds": 5.0
     }
 })
 # Add a measurement integrator
-pipeline.addIntegrator(
+pipeline.add_integrator(
     "my_integrator", {
         "projection_model": {
             "type": "pinhole_camera_projector",
@@ -64,7 +64,7 @@
 stamps_file = os.path.join(measurement_dir, 'timestamps.csv')
 if not os.path.isfile(stamps_file):
     print(f"Could not find timestamp file '{stamps_file}'.")
-with open(stamps_file, 'r') as read_obj:
+with open(stamps_file) as read_obj:
     csv_reader = csv.reader(read_obj)
     for row in csv_reader:
         if row[0] == "ImageID":
@@ -97,7 +97,7 @@
         current_index += 1
         raise SystemExit
     if os.path.isfile(pose_file):
-        with open(pose_file, 'r') as f:
+        with open(pose_file) as f:
             pose_data = [float(x) for x in f.read().split()]
             transform = np.eye(4)
             for row in range(4):
@@ -107,7 +107,7 @@
 
     # Integrate the depth image
     print(f"Integrating measurement {ids[current_index]}")
-    pipeline.runPipeline(["my_integrator"], wave.PosedImage(pose, image))
+    pipeline.run_pipeline(["my_integrator"], wave.PosedImage(pose, image))
 
     current_index += 1
 
diff --git a/examples/python/panoptic_mapping.py b/examples/python/panoptic_mapping.py
index b1ab3d379..92148b591 100644
--- a/examples/python/panoptic_mapping.py
+++ b/examples/python/panoptic_mapping.py
@@ -19,7 +19,7 @@ def __init__(self, params, data_path):
         self.pipeline = wave.Pipeline(self.map)
 
         for operation in params["map_operations"]:
-            self.pipeline.addOperation(operation)
+            self.pipeline.add_operation(operation)
 
         measurement_integrators = params["measurement_integrators"]
         if len(measurement_integrators) != 1:
@@ -27,9 +27,9 @@ def __init__(self, params, data_path):
                   f"Got {len(measurement_integrators)}.")
             raise SystemExit
         self.integrator_name, integrator_params = \
-            next(iter(measurement_integrators.items()))
+            measurement_integrators.popitem()
 
-        self.pipeline.addIntegrator(self.integrator_name, integrator_params)
+        self.pipeline.add_integrator(self.integrator_name, integrator_params)
 
         # Load list of measurements
         stamps_file = os.path.join(self.data_path, 'timestamps.csv')
@@ -38,7 +38,7 @@ def __init__(self, params, data_path):
         self.current_index = 0  # Used to iterate through
         if not os.path.isfile(stamps_file):
             print(f"No timestamp file '{stamps_file}' found.")
-        with open(stamps_file, 'r') as read_obj:
+        with open(stamps_file) as read_obj:
             csv_reader = csv.reader(read_obj)
             for row in csv_reader:
                 if row[0] == "ImageID":
@@ -78,7 +78,7 @@ def integrate_frame(self):
 
         # Load transform
         if os.path.isfile(pose_file):
-            with open(pose_file, 'r') as f:
+            with open(pose_file) as f:
                 pose_data = [float(x) for x in f.read().split()]
                 transform = np.eye(4)
                 for row in range(4):
@@ -86,8 +86,8 @@ def integrate_frame(self):
                         transform[row, col] = pose_data[row * 4 + col]
         pose = wave.Pose(transform)
 
-        self.pipeline.runPipeline([self.integrator_name],
-                                  wave.PosedImage(pose, image))
+        self.pipeline.run_pipeline([self.integrator_name],
+                                   wave.PosedImage(pose, image))
 
         self.current_index += 1
 
diff --git a/examples/python/queries/accelerated_queries.py b/examples/python/queries/accelerated_queries.py
index 5520e422a..84906cb63 100644
--- a/examples/python/queries/accelerated_queries.py
+++ b/examples/python/queries/accelerated_queries.py
@@ -5,13 +5,13 @@
 # Load a map
 your_map = _dummy_objects.example_map()
 
-# Vectorized query for a list of fixed-resolution indices
+# Vectorized query for a list of indices at the highest resolution (height 0)
 indices = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
-values = your_map.getCellValues(indices)
+values = your_map.get_cell_values(indices)
 print(values)
 
-# Vectorized query for a list of node indices
+# Vectorized query for a list of multi-resolution indices (at random heights)
 node_heights = np.random.randint(0, 6, size=(64 * 64 * 32, 1))
 node_indices = np.concatenate((node_heights, indices), axis=1)
-node_values = your_map.getCellValues(node_indices)
+node_values = your_map.get_cell_values(node_indices)
 print(node_values)
diff --git a/examples/python/queries/classification.py b/examples/python/queries/classification.py
index 1c2c017ee..0b37fc859 100644
--- a/examples/python/queries/classification.py
+++ b/examples/python/queries/classification.py
@@ -17,23 +17,23 @@
 
 # In case you would like to convert log odds into probabilities, we provide
 # the following convenience function:
-def logOddsToProbability(log_odds):
+def log_odds_to_probability(log_odds):
     odds = np.exp(log_odds)
     prob = odds / (1.0 + odds)
     return prob
 
 
-occupancy_probability = logOddsToProbability(occupancy_log_odds)
+occupancy_probability = log_odds_to_probability(occupancy_log_odds)
 print(occupancy_probability)
 
 
 # To do the opposite
-def probabilityToLogOdds(probability):
+def probability_to_log_odds(probability):
     odds = probability / (1.0 - probability)
     return np.log(odds)
 
 
-occupancy_log_odds = probabilityToLogOdds(occupancy_probability)
+occupancy_log_odds = probability_to_log_odds(occupancy_probability)
 print(occupancy_log_odds)
 
 # To classify whether a point is estimated to be occupied or free, you need
@@ -47,13 +47,13 @@ def probabilityToLogOdds(probability):
 #       Operating Characteristic curve.
 
 # Once a threshold has been chosen, you can either classify in log space
-is_occupied = kOccupancyThresholdLogOdds < occupancy_log_odds
+is_occupied = kOccupancyThresholdLogOdds <= occupancy_log_odds
 is_free = occupancy_log_odds < kOccupancyThresholdLogOdds
 print(is_occupied)
 print(is_free)
 
 # Or in probability space
-is_occupied = kOccupancyThresholdProb < occupancy_probability
+is_occupied = kOccupancyThresholdProb <= occupancy_probability
 is_free = occupancy_probability < kOccupancyThresholdProb
 print(is_occupied)
 print(is_free)
diff --git a/examples/python/queries/fixed_resolution.py b/examples/python/queries/fixed_resolution.py
index e3c8159d9..e72829680 100644
--- a/examples/python/queries/fixed_resolution.py
+++ b/examples/python/queries/fixed_resolution.py
@@ -8,5 +8,5 @@
 query_index = np.array([0, 0, 0])
 
 # Query the map's value at the given index
-occupancy_log_odds = your_map.getCellValue(query_index)
+occupancy_log_odds = your_map.get_cell_value(query_index)
 print(occupancy_log_odds)
diff --git a/examples/python/queries/multi_resolution.py b/examples/python/queries/multi_resolution.py
index 20b617b7e..0f30215f8 100644
--- a/examples/python/queries/multi_resolution.py
+++ b/examples/python/queries/multi_resolution.py
@@ -17,5 +17,5 @@
                                                query_height)
 
 # Query the node's average occupancy
-occupancy_log_odds = your_map.getCellValue(query_index)
+occupancy_log_odds = your_map.get_cell_value(query_index)
 print(occupancy_log_odds)
diff --git a/examples/python/queries/nearest_neighbor_interpolation.py b/examples/python/queries/nearest_neighbor_interpolation.py
index 6bda45d0f..02a405fbd 100644
--- a/examples/python/queries/nearest_neighbor_interpolation.py
+++ b/examples/python/queries/nearest_neighbor_interpolation.py
@@ -5,7 +5,7 @@
 your_map = _dummy_objects.example_map()
 
 # Declare the point to query [in map frame]
-query_point = np.array([0.4, .5, 0.6])
+query_point = np.array([0.4, 0.5, 0.6])
 
 # Query a single point
 occupancy_log_odds = your_map.interpolateNearest(query_point)
diff --git a/examples/python/queries/trilinear_interpolation.py b/examples/python/queries/trilinear_interpolation.py
index 6b4d79ed8..78525c370 100644
--- a/examples/python/queries/trilinear_interpolation.py
+++ b/examples/python/queries/trilinear_interpolation.py
@@ -5,7 +5,7 @@
 your_map = _dummy_objects.example_map()
 
 # Declare the point to query [in map frame]
-query_point = np.array([0.4, .5, 0.6])
+query_point = np.array([0.4, 0.5, 0.6])
 
 # Query a single point
 occupancy_log_odds = your_map.interpolateTrilinear(query_point)
diff --git a/library/cpp/include/wavemap/core/indexing/ndtree_index.h b/library/cpp/include/wavemap/core/indexing/ndtree_index.h
index 5e10da9d0..3e2d0ff71 100644
--- a/library/cpp/include/wavemap/core/indexing/ndtree_index.h
+++ b/library/cpp/include/wavemap/core/indexing/ndtree_index.h
@@ -22,8 +22,11 @@ struct NdtreeIndex {
   using ChildArray = std::array<NdtreeIndex, kNumChildren>;
 
   //! The node's resolution level in the octree
-  //! @note A height of 0 corresponds to the map’s maximum resolution.
-  //!       The node's size is doubled each time the height is increased by 1.
+  //! @note A height of 0 corresponds to the map’s maximum resolution. In a
+  //!       fully allocated tree, all leaf nodes are at height 0. Increasing
+  //!       the height by 1 doubles the node size along each dimension. The
+  //!       root node corresponds to the map's lowest resolution, and the root
+  //!       node's height matches the configured tree height.
   Element height = 0;
   //! The node's XYZ position in the octree’s grid at the resolution level set
   //! by *height*
diff --git a/library/cpp/include/wavemap/core/map/map_base.h b/library/cpp/include/wavemap/core/map/map_base.h
index 31b054e49..f618e86d6 100644
--- a/library/cpp/include/wavemap/core/map/map_base.h
+++ b/library/cpp/include/wavemap/core/map/map_base.h
@@ -74,11 +74,11 @@ class MapBase {
   //! range specified by its min_log_odds and max_log_odds
   virtual void threshold() = 0;
   //! Free up memory by pruning nodes that are no longer needed
-  //! @note This pruning operation is lossless and does not alter the estimated
-  //!       occupancy posterior.
+  //! @note Implementations of this pruning operation should be lossless and
+  //!       does not alter the estimated occupancy posterior.
   virtual void prune() = 0;
-  //! Similar to prune(), but avoids de-allocating nodes that were recently
-  //! updated and will likely be used again in the near future
+  //! Similar to prune(), but avoids de-allocating nodes that will likely be
+  //! used again in the near future
   virtual void pruneSmart() {
     // NOTE: This method can be overriden by derived classes to provide more
     //       efficient selective pruning strategies. Otherwise, just prune all.
diff --git a/library/cpp/include/wavemap/pipeline/pipeline.h b/library/cpp/include/wavemap/pipeline/pipeline.h
index 2bc5071e5..f7eeb0177 100644
--- a/library/cpp/include/wavemap/pipeline/pipeline.h
+++ b/library/cpp/include/wavemap/pipeline/pipeline.h
@@ -38,7 +38,7 @@ class Pipeline {
   //! Returns true if an integrator with the given name has been registered
   bool hasIntegrator(const std::string& integrator_name) const;
   //! Deregister the integrator with the given name. Returns true if it existed.
-  bool eraseIntegrator(const std::string& integrator_name);
+  bool removeIntegrator(const std::string& integrator_name);
   //! Create and register a new integrator
   IntegratorBase* addIntegrator(const std::string& integrator_name,
                                 const param::Value& integrator_params);
diff --git a/library/cpp/src/pipeline/pipeline.cc b/library/cpp/src/pipeline/pipeline.cc
index 1c554aeb4..e76b37d5f 100644
--- a/library/cpp/src/pipeline/pipeline.cc
+++ b/library/cpp/src/pipeline/pipeline.cc
@@ -12,7 +12,7 @@ bool Pipeline::hasIntegrator(const std::string& integrator_name) const {
   return integrators_.count(integrator_name);
 }
 
-bool Pipeline::eraseIntegrator(const std::string& integrator_name) {
+bool Pipeline::removeIntegrator(const std::string& integrator_name) {
   return integrators_.erase(integrator_name);
 }
 
diff --git a/library/python/src/indices.cc b/library/python/src/indices.cc
index 8c785cfb7..849f9f92e 100644
--- a/library/python/src/indices.cc
+++ b/library/python/src/indices.cc
@@ -14,21 +14,24 @@ void add_index_bindings(nb::module_& m) {
            "position"_a)
       .def_rw("height", &OctreeIndex::height, "height"_a = 0,
               "The node's resolution level in the octree. A height of 0 "
-              "corresponds to the map’s maximum resolution. The node's size is "
-              "doubled each time the height is increased by 1.")
+              "corresponds to the map’s maximum resolution. In a fully "
+              "allocated tree, all leaf nodes are at height 0. Increasing the "
+              "height by 1 doubles the node size along each dimension. The "
+              "root node corresponds to the map's lowest resolution, and the "
+              "root node's height matches the configured tree height.")
       .def_rw("position", &OctreeIndex::position, "position"_a,
               "The node's XYZ position in the octree’s grid at the resolution "
               "level set by *height*.")
-      .def("computeParentIndex",
+      .def("compute_parent_index",
            nb::overload_cast<>(&OctreeIndex::computeParentIndex, nb::const_),
            "Compute the index of the node's direct parent.")
-      .def("computeParentIndex",
+      .def("compute_parent_index",
            nb::overload_cast<OctreeIndex::Element>(
                &OctreeIndex::computeParentIndex, nb::const_),
            "parent_height"_a,
            "Compute the index of the node's parent (or ancestor) at "
            "*parent_height*.")
-      .def("computeChildIndex", &OctreeIndex::computeChildIndex,
+      .def("compute_child_index", &OctreeIndex::computeChildIndex,
            "relative_child_index"_a,
            "Compute the index of the node's n-th child, where n ranges from 0 "
            "to 7.");
diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc
index dfb003e37..e35ac6206 100644
--- a/library/python/src/maps.cc
+++ b/library/python/src/maps.cc
@@ -27,7 +27,7 @@ void add_map_bindings(nb::module_& m) {
            "Free up memory by pruning nodes that are no longer needed. Note "
            "that this pruning operation is lossless and does not alter the "
            "estimated occupancy posterior.")
-      .def("pruneSmart", &MapBase::pruneSmart,
+      .def("prune_smart", &MapBase::pruneSmart,
            "Similar to prune(), but avoids de-allocating nodes that were "
            "recently updated and will likely be used again in the near future.")
       .def("clear", &MapBase::clear, "Erase all cells in the map.")
@@ -51,12 +51,12 @@ void add_map_bindings(nb::module_& m) {
       .def_prop_ro("max_index", &MapBase::getMaxIndex,
                    "Index of the maximum corner of the map's Axis Aligned "
                    "Bounding Box.")
-      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+      .def("get_cell_value", &MapBase::getCellValue, "index"_a,
            "Query the value of the map at a given index.")
-      .def("setCellValue", &MapBase::setCellValue, "index"_a,
+      .def("set_cell_value", &MapBase::setCellValue, "index"_a,
            "new_value"_a
            "Set the value of the map at a given index.")
-      .def("addToCellValue", &MapBase::addToCellValue, "index"_a, "update"_a,
+      .def("add_to_cell_value", &MapBase::addToCellValue, "index"_a, "update"_a,
            "Increment the value of the map at a given index.")
       .def(
           "interpolateNearest",
@@ -100,15 +100,15 @@ void add_map_bindings(nb::module_& m) {
   nb::class_<HashedWaveletOctree, MapBase>(
       m, "HashedWaveletOctree",
       "A class that stores maps using hashed wavelet octrees.")
-      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+      .def("get_cell_value", &MapBase::getCellValue, "index"_a,
            "Query the value of the map at a given index.")
-      .def("getCellValue",
+      .def("get_cell_value",
            nb::overload_cast<const OctreeIndex&>(
                &HashedWaveletOctree::getCellValue, nb::const_),
            "node_index"_a,
            "Query the value of the map at a given octree node index.")
       .def(
-          "getCellValues",
+          "get_cell_values",
           [](const HashedWaveletOctree& self,
              const nb::ndarray<IndexElement, nb::shape<-1, 3>, nb::device::cpu>&
                  indices) {
@@ -137,7 +137,7 @@ void add_map_bindings(nb::module_& m) {
           "Query the map at the given indices, provided as a matrix with one "
           "(x, y, z) index per row.")
       .def(
-          "getCellValues",
+          "get_cell_values",
           [](const HashedWaveletOctree& self,
              const nb::ndarray<IndexElement, nb::shape<-1, 4>, nb::device::cpu>&
                  indices) {
@@ -246,9 +246,9 @@ void add_map_bindings(nb::module_& m) {
   nb::class_<HashedChunkedWaveletOctree, MapBase>(
       m, "HashedChunkedWaveletOctree",
       "A class that stores maps using hashed chunked wavelet octrees.")
-      .def("getCellValue", &MapBase::getCellValue, "index"_a,
+      .def("get_cell_value", &MapBase::getCellValue, "index"_a,
            "Query the value of the map at a given index.")
-      .def("getCellValue",
+      .def("get_cell_value",
            nb::overload_cast<const OctreeIndex&>(
                &HashedChunkedWaveletOctree::getCellValue, nb::const_),
            "node_index"_a,
diff --git a/library/python/src/measurements.cc b/library/python/src/measurements.cc
index 88db19beb..1c51f7052 100644
--- a/library/python/src/measurements.cc
+++ b/library/python/src/measurements.cc
@@ -18,7 +18,7 @@ void add_measurement_bindings(nb::module_& m) {
                                "A class representing poses in 3D space.")
       .def(nb::init<Rotation3D, Vector3D>(), "rotation"_a, "translation"_a)
       .def(nb::init<Transformation3D::TransformationMatrix>(),
-           "transformation_matrix")
+           "transformation_matrix"_a)
       .def("inverse", &Transformation3D::inverse,
            "Compute the pose's inverse.");
 
diff --git a/library/python/src/pipeline.cc b/library/python/src/pipeline.cc
index 9c81fbd1d..a14a6f284 100644
--- a/library/python/src/pipeline.cc
+++ b/library/python/src/pipeline.cc
@@ -16,45 +16,46 @@ void add_pipeline_bindings(nb::module_& m) {
       .def("clear", &Pipeline::clear,
            "Deregister all the pipeline's measurement integrators and map "
            "operations.")
-      .def("hasIntegrator", &Pipeline::hasIntegrator, "integrator_name"_a,
+      .def("has_integrator", &Pipeline::hasIntegrator, "integrator_name"_a,
            "Returns true if an integrator with the given name has been "
            "registered.")
-      .def("eraseIntegrator", &Pipeline::eraseIntegrator, "integrator_name"_a,
+      .def("remove_integrator", &Pipeline::removeIntegrator,
+           "integrator_name"_a,
            "Deregister the integrator with the given name. Returns true if it "
            "existed.")
       .def(
-          "addIntegrator",
+          "add_integrator",
           [](Pipeline& self, const std::string& integrator_name,
              const param::Value& params) -> bool {
             return self.addIntegrator(integrator_name, params);
           },
-          nb::sig("def addIntegrator(self, integrator_name: str, "
+          nb::sig("def add_integrator(self, integrator_name: str, "
                   "integrator_params: dict) -> bool"),
           "integrator_name"_a, "integrator_params"_a,
           "Create and register a new integrator")
-      .def("clearIntegrators", &Pipeline::clearIntegrators,
+      .def("clear_integrators", &Pipeline::clearIntegrators,
            "Deregister all integrators.")
       .def(
-          "addOperation",
+          "add_operation",
           [](Pipeline& self, const param::Value& params) -> bool {
             return self.addOperation(params);
           },
-          nb::sig("def addOperation(self, operation_params: dict) -> bool"),
+          nb::sig("def add_operation(self, operation_params: dict) -> bool"),
           "operation_params"_a, "Create and register a new map operation.")
-      .def("clearOperations", &Pipeline::clearOperations,
+      .def("clear_operations", &Pipeline::clearOperations,
            "Deregister all map operations")
-      .def("runIntegrators", &Pipeline::runIntegrators<PosedPointcloud<>>,
+      .def("run_integrators", &Pipeline::runIntegrators<PosedPointcloud<>>,
            "integrator_names"_a, "posed_pointcloud"_a,
            "Integrate a given pointcloud.")
-      .def("runIntegrators", &Pipeline::runIntegrators<PosedImage<>>,
+      .def("run_integrators", &Pipeline::runIntegrators<PosedImage<>>,
            "integrator_names"_a, "posed_image"_a,
            "Integrate a given depth image.")
-      .def("runOperations", &Pipeline::runOperations, "force_run_all"_a,
+      .def("run_operations", &Pipeline::runOperations, "force_run_all"_a,
            "Run the map operations.")
-      .def("runPipeline", &Pipeline::runPipeline<PosedPointcloud<>>,
+      .def("run_pipeline", &Pipeline::runPipeline<PosedPointcloud<>>,
            "integrator_names"_a, "posed_pointcloud"_a,
            "Integrate a given pointcloud, then run the map operations.")
-      .def("runPipeline", &Pipeline::runPipeline<PosedImage<>>,
+      .def("run_pipeline", &Pipeline::runPipeline<PosedImage<>>,
            "integrator_names"_a, "posed_image"_a,
            "Integrate a given depth image, then run the map operations.");
 }
diff --git a/library/python/test/test_pywavemap.py b/library/python/test/test_pywavemap.py
index d6e689712..d37b8c3c7 100644
--- a/library/python/test/test_pywavemap.py
+++ b/library/python/test/test_pywavemap.py
@@ -20,10 +20,10 @@ def test_batched_fixed_resolution_queries():
     test_map = load_test_map()
 
     cell_indices = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
-    cell_values = test_map.getCellValues(cell_indices)
+    cell_values = test_map.get_cell_values(cell_indices)
     for cell_idx in range(cell_indices.shape[0]):
         cell_index = cell_indices[cell_idx, :]
-        cell_value = test_map.getCellValue(cell_index)
+        cell_value = test_map.get_cell_value(cell_index)
         assert cell_values[cell_idx] == cell_value
 
 
@@ -36,11 +36,11 @@ def test_batched_multi_resolution_queries():
     cell_positions = np.random.randint(-100, 100, size=(64 * 64 * 32, 3))
     cell_heights = np.random.randint(0, 6, size=(64 * 64 * 32, 1))
     cell_indices = np.concatenate((cell_heights, cell_positions), axis=1)
-    cell_values = test_map.getCellValues(cell_indices)
+    cell_values = test_map.get_cell_values(cell_indices)
     for cell_idx in range(cell_positions.shape[0]):
         cell_index = pywavemap.OctreeIndex(cell_heights[cell_idx],
                                            cell_positions[cell_idx, :])
-        cell_value = test_map.getCellValue(cell_index)
+        cell_value = test_map.get_cell_value(cell_index)
         assert cell_values[cell_idx] == cell_value
 
 

From 4b18fab13a560856aff96b4e238e0ead4530be9f Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 13:25:01 +0200
Subject: [PATCH 70/85] Test pywavemap on Ubuntu 22.04 in addition to 20.04

---
 .github/workflows/python.yml | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index 98ddd77b4..c82e7fda8 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -7,7 +7,10 @@ on:
 jobs:
   build:
     name: Build
-    runs-on: ubuntu-20.04
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-22.04]
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
@@ -24,7 +27,10 @@ jobs:
   test:
     name: Test
     needs: build
-    runs-on: ubuntu-20.04
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-22.04]
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4

From 5bf121653cf14cd19af025355cfa660e1916b65d Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 13:27:50 +0200
Subject: [PATCH 71/85] Test C++ library on Ubuntu 22.04 in addition to 20.04

---
 .github/workflows/cpp.yml | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index b99fff330..b2d4b9147 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -7,7 +7,10 @@ on:
 jobs:
   build:
     name: Build
-    runs-on: ubuntu-20.04
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-22.04]
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
@@ -20,7 +23,7 @@ jobs:
       - name: Setup ccache
         uses: hendrikmuhs/ccache-action@v1.2
         with:
-          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-release
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|${{ matrix.os }}-gcc-release
           create-symlink: true
 
       - name: Configure CMake
@@ -37,7 +40,10 @@ jobs:
   test:
     name: Test
     needs: build
-    runs-on: ubuntu-20.04
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-22.04]
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
@@ -50,7 +56,7 @@ jobs:
       - name: Setup ccache
         uses: hendrikmuhs/ccache-action@v1.2
         with:
-          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-release
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|${{ matrix.os }}-gcc-release
           create-symlink: true
 
       - name: Setup GTest

From 0f4eb240336ea208e255c808a8176b650372601f Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 13:45:39 +0200
Subject: [PATCH 72/85] Add Ubuntu 24.04, sanitize on 22.04 to include TSAN,
 and improve README

---
 .github/workflows/cpp.yml    | 14 +++++++-------
 .github/workflows/python.yml |  6 ++++--
 README.md                    |  4 ++--
 3 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index b2d4b9147..150fa1f96 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -10,7 +10,8 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-20.04, ubuntu-22.04]
+        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+      fail-fast: false
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
@@ -43,7 +44,8 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-20.04, ubuntu-22.04]
+        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+      fail-fast: false
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
@@ -171,15 +173,13 @@ jobs:
   sanitize:
     name: Sanitize ${{ matrix.sanitizer.detects }}
     needs: test
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-22.04
     strategy:
       matrix:
         sanitizer:
           - { name: UBSAN, detects: 'undefined behavior' }
           - { name: ASAN, detects: 'addressability and leaks' }
-          # - { name: TSAN, detects: 'data races and deadlocks' }
-          # NOTE: TSAN is disabled until the following bug is resolved:
-          #       https://bugs.launchpad.net/ubuntu/+source/gcc-10/+bug/2029910.
+          - { name: TSAN, detects: 'data races and deadlocks' }
           # NOTE: MSAN is not used for now since it also requires all deps to be
           #       instrumented (recompiled with clang and the MSan flags, LLVM's
           #       stdlib instead of GCCs,...). We therefore use Valgrind to
@@ -197,7 +197,7 @@ jobs:
       - name: Setup ccache
         uses: hendrikmuhs/ccache-action@v1.2
         with:
-          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-20.04-gcc-${{ matrix.sanitizer.name }}
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-22.04-gcc-${{ matrix.sanitizer.name }}
           create-symlink: true
 
       - name: Setup GTest
diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index c82e7fda8..355e7edb2 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -10,7 +10,8 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-20.04, ubuntu-22.04]
+        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+      fail-fast: false
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
@@ -30,7 +31,8 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-20.04, ubuntu-22.04]
+        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+      fail-fast: false
     steps:
       - name: Fetch the package's repository
         uses: actions/checkout@v4
diff --git a/README.md b/README.md
index b5ba152bc..737a112e6 100644
--- a/README.md
+++ b/README.md
@@ -12,13 +12,13 @@ Wavemap achieves state-of-the-art memory and computational efficiency by combini
 
 The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single multi-resolution occupancy grid map.
 
-Wavemap provides [C++](https://ethz-asl.github.io/wavemap/pages/tutorials/cpp) and [Python](https://ethz-asl.github.io/wavemap/pages/tutorials/python) APIs and an interface to [ROS1](https://ethz-asl.github.io/wavemap/pages/tutorials/ros1). The code is extensively tested on Intel, AMD and ARM CPUs. Example Docker files [are available](https://github.com/ethz-asl/wavemap/tree/main/tooling/docker) and documented in the [installation instructions](https://ethz-asl.github.io/wavemap/pages/installation/index). We [welcome contributions](https://ethz-asl.github.io/wavemap/pages/contributing).
+Wavemap provides [C++](https://ethz-asl.github.io/wavemap/pages/tutorials/cpp) and [Python](https://ethz-asl.github.io/wavemap/pages/tutorials/python) APIs and an interface to [ROS1](https://ethz-asl.github.io/wavemap/pages/tutorials/ros1). The code is extensively tested on Intel, AMD and ARM CPUs on Ubuntu 20.04, 22.04 and 24.04. Example Docker files [are available](https://github.com/ethz-asl/wavemap/tree/main/tooling/docker) and documented in the [installation instructions](https://ethz-asl.github.io/wavemap/pages/installation/index). We [welcome contributions](https://ethz-asl.github.io/wavemap/pages/contributing).
 
 ⭐ If you find wavemap useful, star it on GitHub to get notified of new releases!
 
 
 ## Documentation
-The framework's documentation is hosted on [GitHub Pages](https://ethz-asl.github.io/wavemap/).
+The framework's documentation is available on [GitHub Pages](https://ethz-asl.github.io/wavemap/) for easy online access. A PDF version of each release’s documentation can also be found in the respective [release notes](https://github.com/ethz-asl/wavemap/releases).
 
 ### Table of contents
 * [Installation](https://ethz-asl.github.io/wavemap/pages/installation)

From 97cdc9d70a9c50c450b36702b46a2de9937454c2 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 14:00:48 +0200
Subject: [PATCH 73/85] Only explicitly upgrade pip on Ubuntu 20.04

---
 .github/workflows/python.yml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index 355e7edb2..0deefc196 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -20,6 +20,7 @@ jobs:
         uses: actions/setup-python@v5
 
       - name: Upgrade pip
+        if: matrix.os == 'ubuntu-20.04'
         run: python -m pip install --upgrade pip
 
       - name: Build

From c9e5eee1b91fdaf7310abdb780db1c2bc8875a92 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 14:13:27 +0200
Subject: [PATCH 74/85] Use virtual environment to avoid pip errors on newer
 Ubuntu versions

---
 .github/workflows/python.yml | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index 0deefc196..f607fdd73 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -23,6 +23,12 @@ jobs:
         if: matrix.os == 'ubuntu-20.04'
         run: python -m pip install --upgrade pip
 
+      - name: Activate virtual environment
+        run: |
+          python -m venv my-venv
+          source my-venv/bin/activate
+          echo PATH=$PATH >> $GITHUB_ENV
+
       - name: Build
         run: python -m pip install -v ./library/python/
 
@@ -42,8 +48,15 @@ jobs:
         uses: actions/setup-python@v5
 
       - name: Upgrade pip
+        if: matrix.os == 'ubuntu-20.04'
         run: python -m pip install --upgrade pip
 
+      - name: Activate virtual environment
+        run: |
+          python -m venv my-venv
+          source my-venv/bin/activate
+          echo PATH=$PATH >> $GITHUB_ENV
+
       - name: Test
         run: |
           python -m pip install -v './library/python[test]'

From b775d992ba2e9152e11fe225dee2841b0055e759 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 14:16:11 +0200
Subject: [PATCH 75/85] Upgrade pip version in virtual environment

---
 .github/workflows/python.yml | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index f607fdd73..5b3008455 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -19,16 +19,16 @@ jobs:
       - name: Setup Python
         uses: actions/setup-python@v5
 
-      - name: Upgrade pip
-        if: matrix.os == 'ubuntu-20.04'
-        run: python -m pip install --upgrade pip
-
       - name: Activate virtual environment
         run: |
           python -m venv my-venv
           source my-venv/bin/activate
           echo PATH=$PATH >> $GITHUB_ENV
 
+      - name: Upgrade pip
+        if: matrix.os == 'ubuntu-20.04'
+        run: python -m pip install --upgrade pip
+
       - name: Build
         run: python -m pip install -v ./library/python/
 
@@ -47,16 +47,16 @@ jobs:
       - name: Setup Python
         uses: actions/setup-python@v5
 
-      - name: Upgrade pip
-        if: matrix.os == 'ubuntu-20.04'
-        run: python -m pip install --upgrade pip
-
       - name: Activate virtual environment
         run: |
           python -m venv my-venv
           source my-venv/bin/activate
           echo PATH=$PATH >> $GITHUB_ENV
 
+      - name: Upgrade pip
+        if: matrix.os == 'ubuntu-20.04'
+        run: python -m pip install --upgrade pip
+
       - name: Test
         run: |
           python -m pip install -v './library/python[test]'

From 32036f86d673f83c1bcfa0c70bf172f7bc930e0b Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 14:27:34 +0200
Subject: [PATCH 76/85] Setup python-version to suppress warnings

---
 .github/workflows/python.yml | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml
index 5b3008455..49d192a97 100644
--- a/.github/workflows/python.yml
+++ b/.github/workflows/python.yml
@@ -18,6 +18,8 @@ jobs:
 
       - name: Setup Python
         uses: actions/setup-python@v5
+        with:
+          python-version: '>=3.8'
 
       - name: Activate virtual environment
         run: |
@@ -46,6 +48,8 @@ jobs:
 
       - name: Setup Python
         uses: actions/setup-python@v5
+        with:
+          python-version: '>=3.8'
 
       - name: Activate virtual environment
         run: |

From a73cb72eff472d7fae1e595c2a690dec1baed224 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 14:31:21 +0200
Subject: [PATCH 77/85] Make ROS1 CI workflow less verbose

---
 .github/workflows/ros1.yml | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml
index e409c0012..67bb300bb 100644
--- a/.github/workflows/ros1.yml
+++ b/.github/workflows/ros1.yml
@@ -34,7 +34,7 @@ jobs:
         uses: docker/setup-buildx-action@v3
 
       - name: Log in to ${{ env.DOCKER_REGISTRY }} registry
-        uses: docker/login-action@65b78e6e13532edd9afa3aa52ac7964289d1a9c1
+        uses: docker/login-action@v3.3.0
         with:
           registry: ${{ env.DOCKER_REGISTRY }}
           username: ${{ github.actor }}
@@ -42,6 +42,9 @@ jobs:
 
       - name: Build the image
         uses: docker/build-push-action@v6
+        env:
+          DOCKER_BUILD_SUMMARY: false
+          DOCKER_BUILD_RECORD_UPLOAD: false
         with:
           context: ${{ github.repository }}
           file: ${{ github.repository }}/tooling/docker/ros1/full.Dockerfile
@@ -61,6 +64,9 @@ jobs:
 
       - name: Push the image
         uses: docker/build-push-action@v6
+        env:
+          DOCKER_BUILD_SUMMARY: false
+          DOCKER_BUILD_RECORD_UPLOAD: false
         with:
           context: ${{ github.repository }}
           file: ${{ github.repository }}/tooling/docker/ros1/full.Dockerfile

From 209dc70334d593599b846602a3d6c79bcb4e7ce0 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 14:47:37 +0200
Subject: [PATCH 78/85] Address deprecation and suppress unused result warnings

---
 library/cpp/cmake/wavemap-extras.cmake                       | 3 ++-
 .../wavemap/core/utils/math/approximate_trigonometry.h       | 5 ++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/library/cpp/cmake/wavemap-extras.cmake b/library/cpp/cmake/wavemap-extras.cmake
index 5237acde2..33aa2e3cb 100644
--- a/library/cpp/cmake/wavemap-extras.cmake
+++ b/library/cpp/cmake/wavemap-extras.cmake
@@ -32,7 +32,8 @@ function(set_wavemap_target_properties target)
   set_target_properties(${target} PROPERTIES POSITION_INDEPENDENT_CODE ON)
   target_compile_options(${target} PUBLIC -march=native)
   target_compile_options(${target} PRIVATE
-      -Wall -Wextra -Wpedantic -Wno-deprecated-copy -Wno-class-memaccess)
+      -Wall -Wextra -Wpedantic
+      -Wno-unused-result -Wno-deprecated-copy -Wno-class-memaccess)
 
   # General C++ defines
   target_compile_definitions(${target} PUBLIC EIGEN_INITIALIZE_MATRICES_BY_NAN)
diff --git a/library/cpp/include/wavemap/core/utils/math/approximate_trigonometry.h b/library/cpp/include/wavemap/core/utils/math/approximate_trigonometry.h
index 541dba076..38337042e 100644
--- a/library/cpp/include/wavemap/core/utils/math/approximate_trigonometry.h
+++ b/library/cpp/include/wavemap/core/utils/math/approximate_trigonometry.h
@@ -7,7 +7,7 @@
 #include "wavemap/core/common.h"
 
 namespace wavemap::approximate {
-struct atan : public std::unary_function<FloatingPoint, FloatingPoint> {
+struct atan {
   FloatingPoint operator()(FloatingPoint x) const {
     // Copyright (c) 2021 Francesco Mazzoli <f@mazzo.li>
     //
@@ -40,8 +40,7 @@ struct atan : public std::unary_function<FloatingPoint, FloatingPoint> {
   }
 };
 
-struct atan2
-    : public std::binary_function<FloatingPoint, FloatingPoint, FloatingPoint> {
+struct atan2 {
   static constexpr FloatingPoint kWorstCaseError = 1.908e-6f;
 
   FloatingPoint operator()(FloatingPoint y, FloatingPoint x) const {

From 62a9713394a1ead726bac904397af8e8f0633820 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 15:22:21 +0200
Subject: [PATCH 79/85] Switch UBSAN and ASAN back to 20.04 as UBSAN doesn't
 work on 22.04

---
 .github/workflows/cpp.yml | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index 150fa1f96..713aeaeaa 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -10,7 +10,7 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+        os: [ ubuntu-20.04, ubuntu-22.04, ubuntu-24.04 ]
       fail-fast: false
     steps:
       - name: Fetch the package's repository
@@ -44,7 +44,7 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-20.04, ubuntu-22.04, ubuntu-24.04]
+        os: [ ubuntu-20.04, ubuntu-22.04, ubuntu-24.04 ]
       fail-fast: false
     steps:
       - name: Fetch the package's repository
@@ -173,13 +173,15 @@ jobs:
   sanitize:
     name: Sanitize ${{ matrix.sanitizer.detects }}
     needs: test
-    runs-on: ubuntu-22.04
+    runs-on: ${{ matrix.sanitizer.os }}
     strategy:
       matrix:
         sanitizer:
-          - { name: UBSAN, detects: 'undefined behavior' }
-          - { name: ASAN, detects: 'addressability and leaks' }
-          - { name: TSAN, detects: 'data races and deadlocks' }
+          - { name: UBSAN, detects: 'undefined behavior', os: ubuntu-20.04 }
+          - { name: ASAN, detects: 'addressability and leaks', os: ubuntu-20.04 }
+          - { name: TSAN, detects: 'data races and deadlocks', os: ubuntu-22.04 }
+          # NOTE: We run TSAN on Ubuntu 22.04 since it's broken on 20.04, see:
+          #       https://bugs.launchpad.net/ubuntu/+source/gcc-10/+bug/2029910.
           # NOTE: MSAN is not used for now since it also requires all deps to be
           #       instrumented (recompiled with clang and the MSan flags, LLVM's
           #       stdlib instead of GCCs,...). We therefore use Valgrind to
@@ -197,7 +199,7 @@ jobs:
       - name: Setup ccache
         uses: hendrikmuhs/ccache-action@v1.2
         with:
-          key: ${{ secrets.CCACHE_CACHE_VERSION }}|ubuntu-22.04-gcc-${{ matrix.sanitizer.name }}
+          key: ${{ secrets.CCACHE_CACHE_VERSION }}|${{ matrix.sanitizer.os }}-gcc-${{ matrix.sanitizer.name }}
           create-symlink: true
 
       - name: Setup GTest

From 450d9ae42c676b50572f1745d0c92f6365d9354f Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 15:33:49 +0200
Subject: [PATCH 80/85] Fix out of bounds access in Haar coefficients print
 method

---
 .../wavemap/core/map/cell_types/haar_coefficients.h       | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/library/cpp/include/wavemap/core/map/cell_types/haar_coefficients.h b/library/cpp/include/wavemap/core/map/cell_types/haar_coefficients.h
index 6de02e55a..1ba331d41 100644
--- a/library/cpp/include/wavemap/core/map/cell_types/haar_coefficients.h
+++ b/library/cpp/include/wavemap/core/map/cell_types/haar_coefficients.h
@@ -56,8 +56,6 @@ struct HaarCoefficients {
       return *this;
     }
     std::string toString() const {
-      // TODO(victorr): Check if the order of the labels matches the transform's
-      //                implementation
       std::stringstream ss;
       ss << "[";
       for (int coeff_idx = 1; coeff_idx <= kNumDetailCoefficients;
@@ -65,7 +63,7 @@ struct HaarCoefficients {
         for (int dim_idx = 0; dim_idx < kDim; ++dim_idx) {
           ss << (bit_ops::is_bit_set(coeff_idx, dim_idx) ? "H" : "L");
         }
-        ss << " = " << this->operator[](coeff_idx) << ", ";
+        ss << " = " << this->operator[](coeff_idx - 1) << ", ";
       }
       ss << "\b\b]";
       return ss.str();
@@ -104,14 +102,14 @@ struct HaarCoefficients {
       return {lhs.scale + rhs.scale, lhs.details + rhs.details};
     }
     Parent& operator+=(const Parent& rhs) {
-      *this = *this + rhs.coefficients;
+      *this = *this + rhs;
       return *this;
     }
     friend Parent operator-(const Parent& lhs, const Parent& rhs) {
       return {lhs.scale - rhs.scale, lhs.details - rhs.details};
     }
     Parent& operator-=(const Parent& rhs) {
-      *this = *this - rhs.coefficients;
+      *this = *this - rhs;
       return *this;
     }
     friend Parent operator*(ValueType lhs, const Parent& rhs) {

From 8a4889d97c71d4807599852d22ba6403d1c7094c Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 16:57:40 +0200
Subject: [PATCH 81/85] Set interpolation mode through argument, not
 alternative method names

---
 docs/python_api/index.rst                     |   2 +
 .../queries/nearest_neighbor_interpolation.py |   6 +-
 .../python/queries/trilinear_interpolation.py |   6 +-
 library/python/src/maps.cc                    | 186 +++++++++---------
 library/python/src/pywavemap/__init__.py      |   3 +-
 library/python/test/test_pywavemap.py         |  17 +-
 6 files changed, 114 insertions(+), 106 deletions(-)

diff --git a/docs/python_api/index.rst b/docs/python_api/index.rst
index c77d011c8..0066b42f1 100644
--- a/docs/python_api/index.rst
+++ b/docs/python_api/index.rst
@@ -14,6 +14,8 @@ Python API
 .. autoclass:: pywavemap.HashedChunkedWaveletOctree
     :show-inheritance:
     :members:
+.. autoclass:: pywavemap.InterpolationMode
+    :members:
 
 .. autoclass:: pywavemap.OctreeIndex
     :members:
diff --git a/examples/python/queries/nearest_neighbor_interpolation.py b/examples/python/queries/nearest_neighbor_interpolation.py
index 02a405fbd..791fb907f 100644
--- a/examples/python/queries/nearest_neighbor_interpolation.py
+++ b/examples/python/queries/nearest_neighbor_interpolation.py
@@ -1,4 +1,5 @@
 import numpy as np
+from pywavemap import InterpolationMode
 import _dummy_objects
 
 # Load a map
@@ -8,10 +9,11 @@
 query_point = np.array([0.4, 0.5, 0.6])
 
 # Query a single point
-occupancy_log_odds = your_map.interpolateNearest(query_point)
+occupancy_log_odds = your_map.interpolate(query_point,
+                                          InterpolationMode.NEAREST)
 print(occupancy_log_odds)
 
 # Vectorized query for a list of points
 points = np.random.random(size=(64 * 64 * 32, 3))
-points_log_odds = your_map.interpolateNearest(points)
+points_log_odds = your_map.interpolate(points, InterpolationMode.NEAREST)
 print(points_log_odds)
diff --git a/examples/python/queries/trilinear_interpolation.py b/examples/python/queries/trilinear_interpolation.py
index 78525c370..412822b4a 100644
--- a/examples/python/queries/trilinear_interpolation.py
+++ b/examples/python/queries/trilinear_interpolation.py
@@ -1,4 +1,5 @@
 import numpy as np
+from pywavemap import InterpolationMode
 import _dummy_objects
 
 # Load a map
@@ -8,10 +9,11 @@
 query_point = np.array([0.4, 0.5, 0.6])
 
 # Query a single point
-occupancy_log_odds = your_map.interpolateTrilinear(query_point)
+occupancy_log_odds = your_map.interpolate(query_point,
+                                          InterpolationMode.TRILINEAR)
 print(occupancy_log_odds)
 
 # Vectorized query for a list of points
 points = np.random.random(size=(64 * 64 * 32, 3))
-points_log_odds = your_map.interpolateTrilinear(points)
+points_log_odds = your_map.interpolate(points, InterpolationMode.TRILINEAR)
 print(points_log_odds)
diff --git a/library/python/src/maps.cc b/library/python/src/maps.cc
index e35ac6206..036e956bf 100644
--- a/library/python/src/maps.cc
+++ b/library/python/src/maps.cc
@@ -15,6 +15,14 @@ using namespace nb::literals;  // NOLINT
 
 namespace wavemap {
 void add_map_bindings(nb::module_& m) {
+  enum class InterpolationMode { kNearest, kTrilinear };
+
+  nb::enum_<InterpolationMode>(m, "InterpolationMode")
+      .value("NEAREST", InterpolationMode::kNearest,
+             "Look up the value of the nearest map cell.")
+      .value("TRILINEAR", InterpolationMode::kTrilinear,
+             "Interpolate linearly along each map axis.");
+
   nb::class_<MapBase>(m, "Map", "Base class for wavemap maps.")
       .def_prop_ro("empty", &MapBase::empty, "Whether the map is empty.")
       .def_prop_ro("size", &MapBase::size,
@@ -59,20 +67,21 @@ void add_map_bindings(nb::module_& m) {
       .def("add_to_cell_value", &MapBase::addToCellValue, "index"_a, "update"_a,
            "Increment the value of the map at a given index.")
       .def(
-          "interpolateNearest",
-          [](const MapBase& self, const Point3D& position) {
-            return interpolate::nearestNeighbor(self, position);
-          },
-          "position"_a,
-          "Query the map's value at a point using nearest neighbor "
-          "interpolation.")
-      .def(
-          "interpolateTrilinear",
-          [](const MapBase& self, const Point3D& position) {
-            return interpolate::trilinear(self, position);
+          "interpolate",
+          [](const MapBase& self, const Point3D& position,
+             InterpolationMode mode) {
+            switch (mode) {
+              case InterpolationMode::kNearest:
+                return interpolate::nearestNeighbor(self, position);
+              case InterpolationMode::kTrilinear:
+                return interpolate::trilinear(self, position);
+              default:
+                throw nb::type_error("Unknown interpolation mode.");
+            }
           },
-          "position"_a,
-          "Query the map's value at a point using trilinear interpolation.")
+          "position"_a, "mode"_a = InterpolationMode::kTrilinear,
+          "Query the map's value at a point, using the specified interpolation "
+          "mode.")
       .def_static(
           "create",
           [](const param::Value& params) -> std::shared_ptr<MapBase> {
@@ -117,18 +126,18 @@ void add_map_bindings(nb::module_& m) {
             // Create nb::ndarray view for efficient access to the query indices
             const auto index_view = indices.view();
             const auto num_queries = index_view.shape(0);
-            // Allocate and populate raw results array
+            // Create the raw results array and wrap it in a Python capsule that
+            // deallocates it when all references to it expire
             auto* results = new float[num_queries];
+            nb::capsule owner(results, [](void* p) noexcept {
+              delete[] reinterpret_cast<float*>(p);
+            });
+            // Compute the interpolated values
             for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
               results[query_idx] = query_accelerator.getCellValue(
                   {index_view(query_idx, 0), index_view(query_idx, 1),
                    index_view(query_idx, 2)});
             }
-            // Create Python capsule that deallocates the results array when
-            // all references to it expire
-            nb::capsule owner(results, [](void* p) noexcept {
-              delete[] reinterpret_cast<float*>(p);
-            });
             // Return results as numpy array
             return nb::ndarray<nb::numpy, float>{
                 results, {num_queries, 1u}, owner};
@@ -146,8 +155,13 @@ void add_map_bindings(nb::module_& m) {
             // Create nb::ndarray view for efficient access to the query indices
             auto index_view = indices.view();
             const auto num_queries = index_view.shape(0);
-            // Allocate and populate raw results array
+            // Create the raw results array and wrap it in a Python capsule that
+            // deallocates it when all references to it expire
             auto* results = new float[num_queries];
+            nb::capsule owner(results, [](void* p) noexcept {
+              delete[] reinterpret_cast<float*>(p);
+            });
+            // Compute the interpolated values
             for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
               const OctreeIndex node_index{
                   index_view(query_idx, 0),
@@ -155,11 +169,6 @@ void add_map_bindings(nb::module_& m) {
                    index_view(query_idx, 3)}};
               results[query_idx] = query_accelerator.getCellValue(node_index);
             }
-            // Create Python capsule that deallocates the results array when
-            // all references to it expire
-            nb::capsule owner(results, [](void* p) noexcept {
-              delete[] reinterpret_cast<float*>(p);
-            });
             // Return results as numpy array
             return nb::ndarray<nb::numpy, float>{
                 results, {num_queries, 1u}, owner};
@@ -168,80 +177,68 @@ void add_map_bindings(nb::module_& m) {
           "Query the map at the given node indices, provided as a matrix with "
           "one (height, x, y, z) node index per row.")
       .def(
-          "interpolateNearest",
-          [](const MapBase& self, const Point3D& position) {
-            return interpolate::nearestNeighbor(self, position);
-          },
-          "position"_a,
-          "Query the map's value at a point using nearest neighbor "
-          "interpolation.")
-      .def(
-          "interpolateTrilinear",
-          [](const MapBase& self, const Point3D& position) {
-            return interpolate::trilinear(self, position);
-          },
-          "position"_a,
-          "Query the map's value at a point using trilinear interpolation.")
-      .def(
-          "interpolateNearest",
-          [](const HashedWaveletOctree& self,
-             const nb::ndarray<FloatingPoint, nb::shape<-1, 3>,
-                               nb::device::cpu>& positions) {
-            // Create a query accelerator
-            QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
-            // Create nb::ndarray view for efficient access to the query points
-            const auto positions_view = positions.view();
-            const auto num_queries = positions_view.shape(0);
-            // Allocate and populate raw results array
-            auto* results = new float[num_queries];
-            for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
-              results[query_idx] = interpolate::nearestNeighbor(
-                  query_accelerator,
-                  {positions_view(query_idx, 0), positions_view(query_idx, 1),
-                   positions_view(query_idx, 2)});
+          "interpolate",
+          [](const MapBase& self, const Point3D& position,
+             InterpolationMode mode) {
+            switch (mode) {
+              case InterpolationMode::kNearest:
+                return interpolate::nearestNeighbor(self, position);
+              case InterpolationMode::kTrilinear:
+                return interpolate::trilinear(self, position);
+              default:
+                throw nb::type_error("Unknown interpolation mode.");
             }
-            // Create Python capsule that deallocates the results array when
-            // all references to it expire
-            nb::capsule owner(results, [](void* p) noexcept {
-              delete[] reinterpret_cast<float*>(p);
-            });
-            // Return results as numpy array
-            return nb::ndarray<nb::numpy, float>{
-                results, {num_queries, 1u}, owner};
           },
-          "position_list"_a,
-          "Query the map's value at the given points using nearest neighbor "
-          "interpolation.")
+          "position"_a, "mode"_a = InterpolationMode::kTrilinear,
+          "Query the map's value at a point, using the specified interpolation "
+          "mode.")
       .def(
-          "interpolateTrilinear",
+          "interpolate",
           [](const HashedWaveletOctree& self,
              const nb::ndarray<FloatingPoint, nb::shape<-1, 3>,
-                               nb::device::cpu>& positions) {
+                               nb::device::cpu>& positions,
+             InterpolationMode mode) {
             // Create a query accelerator
             QueryAccelerator<HashedWaveletOctree> query_accelerator{self};
             // Create nb::ndarray view for efficient access to the query points
             const auto positions_view = positions.view();
             const auto num_queries = positions_view.shape(0);
-            // Allocate and populate raw results array
+            // Create the raw results array and wrap it in a Python capsule that
+            // deallocates it when all references to it expire
             auto* results = new float[num_queries];
-            for (size_t query_idx = 0; query_idx < num_queries; ++query_idx) {
-              results[query_idx] = interpolate::trilinear(
-                  query_accelerator,
-                  {positions_view(query_idx, 0), positions_view(query_idx, 1),
-                   positions_view(query_idx, 2)});
-            }
-            // Create Python capsule that deallocates the results array when
-            // all references to it expire
             nb::capsule owner(results, [](void* p) noexcept {
               delete[] reinterpret_cast<float*>(p);
             });
+            // Compute the interpolated values
+            switch (mode) {
+              case InterpolationMode::kNearest:
+                for (size_t query_idx = 0; query_idx < num_queries;
+                     ++query_idx) {
+                  results[query_idx] = interpolate::nearestNeighbor(
+                      query_accelerator, {positions_view(query_idx, 0),
+                                          positions_view(query_idx, 1),
+                                          positions_view(query_idx, 2)});
+                }
+                break;
+              case InterpolationMode::kTrilinear:
+                for (size_t query_idx = 0; query_idx < num_queries;
+                     ++query_idx) {
+                  results[query_idx] = interpolate::trilinear(
+                      query_accelerator, {positions_view(query_idx, 0),
+                                          positions_view(query_idx, 1),
+                                          positions_view(query_idx, 2)});
+                }
+                break;
+              default:
+                throw nb::type_error("Unknown interpolation mode.");
+            }
             // Return results as numpy array
             return nb::ndarray<nb::numpy, float>{
                 results, {num_queries, 1u}, owner};
           },
-          "position_list"_a,
-          "Query the map's value at the given points using trilinear "
-          "interpolation.");
+          "position_list"_a, "mode"_a = InterpolationMode::kTrilinear,
+          "Query the map's value at the given points, using the specified "
+          "interpolation mode.");
 
   nb::class_<HashedChunkedWaveletOctree, MapBase>(
       m, "HashedChunkedWaveletOctree",
@@ -254,19 +251,20 @@ void add_map_bindings(nb::module_& m) {
            "node_index"_a,
            "Query the value of the map at a given octree node index.")
       .def(
-          "interpolateNearest",
-          [](const MapBase& self, const Point3D& position) {
-            return interpolate::nearestNeighbor(self, position);
-          },
-          "position"_a,
-          "Query the map's value at a point using nearest neighbor "
-          "interpolation.")
-      .def(
-          "interpolateTrilinear",
-          [](const MapBase& self, const Point3D& position) {
-            return interpolate::trilinear(self, position);
+          "interpolate",
+          [](const MapBase& self, const Point3D& position,
+             InterpolationMode mode) {
+            switch (mode) {
+              case InterpolationMode::kNearest:
+                return interpolate::nearestNeighbor(self, position);
+              case InterpolationMode::kTrilinear:
+                return interpolate::trilinear(self, position);
+              default:
+                throw nb::type_error("Unknown interpolation mode.");
+            }
           },
-          "position"_a,
-          "Query the map's value at a point using trilinear interpolation.");
+          "position"_a, "mode"_a = InterpolationMode::kTrilinear,
+          "Query the map's value at a point, using the specified interpolation "
+          "mode.");
 }
 }  // namespace wavemap
diff --git a/library/python/src/pywavemap/__init__.py b/library/python/src/pywavemap/__init__.py
index fd40eb74f..f1db669c3 100644
--- a/library/python/src/pywavemap/__init__.py
+++ b/library/python/src/pywavemap/__init__.py
@@ -6,7 +6,8 @@
 from ._pywavemap_bindings import (Rotation, Pose, Pointcloud, PosedPointcloud,
                                   Image, PosedImage)
 from ._pywavemap_bindings import (Map, HashedWaveletOctree,
-                                  HashedChunkedWaveletOctree)
+                                  HashedChunkedWaveletOctree,
+                                  InterpolationMode)
 from ._pywavemap_bindings import Pipeline
 
 # Binding submodules
diff --git a/library/python/test/test_pywavemap.py b/library/python/test/test_pywavemap.py
index d37b8c3c7..9b06bdecc 100644
--- a/library/python/test/test_pywavemap.py
+++ b/library/python/test/test_pywavemap.py
@@ -29,7 +29,7 @@ def test_batched_fixed_resolution_queries():
 
 def test_batched_multi_resolution_queries():
     import numpy as np
-    import pywavemap
+    import pywavemap as wave
 
     test_map = load_test_map()
 
@@ -38,33 +38,36 @@ def test_batched_multi_resolution_queries():
     cell_indices = np.concatenate((cell_heights, cell_positions), axis=1)
     cell_values = test_map.get_cell_values(cell_indices)
     for cell_idx in range(cell_positions.shape[0]):
-        cell_index = pywavemap.OctreeIndex(cell_heights[cell_idx],
-                                           cell_positions[cell_idx, :])
+        cell_index = wave.OctreeIndex(cell_heights[cell_idx],
+                                      cell_positions[cell_idx, :])
         cell_value = test_map.get_cell_value(cell_index)
         assert cell_values[cell_idx] == cell_value
 
 
 def test_batched_nearest_neighbor_interpolation():
     import numpy as np
+    from pywavemap import InterpolationMode
 
     test_map = load_test_map()
 
     points = np.random.random(size=(64 * 64 * 32, 3))
-    points_log_odds = test_map.interpolateNearest(points)
+    points_log_odds = test_map.interpolate(points, InterpolationMode.NEAREST)
     for point_idx in range(points.shape[0]):
         point = points[point_idx, :]
-        point_log_odds = test_map.interpolateNearest(point)
+        point_log_odds = test_map.interpolate(point, InterpolationMode.NEAREST)
         assert points_log_odds[point_idx] == point_log_odds
 
 
 def test_batched_trilinear_interpolation():
     import numpy as np
+    from pywavemap import InterpolationMode
 
     test_map = load_test_map()
 
     points = np.random.random(size=(64 * 64 * 32, 3))
-    points_log_odds = test_map.interpolateTrilinear(points)
+    points_log_odds = test_map.interpolate(points, InterpolationMode.TRILINEAR)
     for point_idx in range(points.shape[0]):
         point = points[point_idx, :]
-        point_log_odds = test_map.interpolateTrilinear(point)
+        point_log_odds = test_map.interpolate(point,
+                                              InterpolationMode.TRILINEAR)
         assert points_log_odds[point_idx] == point_log_odds

From 8243f962604d7a982d1c95c821664019c3b3029b Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 17:49:42 +0200
Subject: [PATCH 82/85] Extend release management script to handle Python pkgs

---
 .../pages/installation/{cmake.rst => cpp.rst} |  0
 docs/pages/installation/index.rst             |  2 +-
 docs/pages/tutorials/cpp.rst                  |  2 +-
 examples/python/CHANGELOG.rst                 |  3 +
 library/python/CHANGELOG.rst                  |  3 +
 library/python/CMakeLists.txt                 |  2 +-
 library/python/README.md                      |  0
 library/python/pyproject.toml                 |  2 +-
 tooling/scripts/prepare_release.py            | 71 +++++++++++++++----
 9 files changed, 67 insertions(+), 18 deletions(-)
 rename docs/pages/installation/{cmake.rst => cpp.rst} (100%)
 create mode 100644 examples/python/CHANGELOG.rst
 create mode 100644 library/python/CHANGELOG.rst
 delete mode 100644 library/python/README.md

diff --git a/docs/pages/installation/cmake.rst b/docs/pages/installation/cpp.rst
similarity index 100%
rename from docs/pages/installation/cmake.rst
rename to docs/pages/installation/cpp.rst
diff --git a/docs/pages/installation/index.rst b/docs/pages/installation/index.rst
index 323ecb042..720305dc0 100644
--- a/docs/pages/installation/index.rst
+++ b/docs/pages/installation/index.rst
@@ -13,6 +13,6 @@ For roboticists working with ROS, we provide packages that tightly integrate wav
     :caption: Installation types
     :maxdepth: 1
 
-    cmake
+    cpp
     python
     ros1
diff --git a/docs/pages/tutorials/cpp.rst b/docs/pages/tutorials/cpp.rst
index 69827030c..43ab08f12 100644
--- a/docs/pages/tutorials/cpp.rst
+++ b/docs/pages/tutorials/cpp.rst
@@ -10,7 +10,7 @@ In this tutorial, we illustrate how you can use wavemap's C++ API in your own pr
 
 CMake target setup
 ******************
-Once you included wavemap's C++ library in your CMake project, for example by following our :doc:`installation instructions <../installation/cmake>`, the last remaining step to start using it is to configure your CMake target (e.g. library or executable) to use it.
+Once you included wavemap's C++ library in your CMake project, for example by following our :doc:`installation instructions <../installation/cpp>`, the last remaining step to start using it is to configure your CMake target (e.g. library or executable) to use it.
 
 Wavemap's C++ library contains three main components:
 
diff --git a/examples/python/CHANGELOG.rst b/examples/python/CHANGELOG.rst
new file mode 100644
index 000000000..f9b6608ea
--- /dev/null
+++ b/examples/python/CHANGELOG.rst
@@ -0,0 +1,3 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package wavemap_examples_python
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
diff --git a/library/python/CHANGELOG.rst b/library/python/CHANGELOG.rst
new file mode 100644
index 000000000..3e9303886
--- /dev/null
+++ b/library/python/CHANGELOG.rst
@@ -0,0 +1,3 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package pywavemap
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 38a7ab870..074d9d053 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.18)
-project(pywavemap LANGUAGES CXX)
+project(pywavemap VERSION 2.0.1 LANGUAGES CXX)
 
 # Warn if the user invokes CMake directly
 if (NOT SKBUILD AND NOT $ENV{CLION_IDE})
diff --git a/library/python/README.md b/library/python/README.md
deleted file mode 100644
index e69de29bb..000000000
diff --git a/library/python/pyproject.toml b/library/python/pyproject.toml
index 7d98f6b5d..72ec86443 100644
--- a/library/python/pyproject.toml
+++ b/library/python/pyproject.toml
@@ -6,7 +6,7 @@ build-backend = "scikit_build_core.build"
 name = "pywavemap"
 version = "2.0.0"
 description = "A fast, efficient and accurate multi-resolution, multi-sensor 3D occupancy mapping framework."
-readme = "README.md"
+readme = "../../README.md"
 requires-python = ">=3.8"
 authors = [
     { name = "Victor Reijgwart", email = "victorr@ethz.ch" },
diff --git a/tooling/scripts/prepare_release.py b/tooling/scripts/prepare_release.py
index 163b8d645..d34b8c63f 100755
--- a/tooling/scripts/prepare_release.py
+++ b/tooling/scripts/prepare_release.py
@@ -59,10 +59,17 @@ def bump_version(version, level='patch'):
 class PkgType(Enum):
 
     def __str__(self):
-        return {PkgType.CPP: "C++", PkgType.ROS1: "ROS1"}[self]
+        return {
+            PkgType.CPP: "C++",
+            PkgType.PYTHON_BINDINGS: "Python",
+            PkgType.ROS1: "ROS1",
+            PkgType.PYTHON: "Python"
+        }[self]
 
     CPP = 1
-    ROS1 = 2
+    PYTHON_BINDINGS = 2
+    ROS1 = 3
+    PYTHON = 4
 
 
 # Class used to specify a package in our repository
@@ -85,9 +92,9 @@ def draft_release_notes():
     out += "\n"
 
     out += "### Libraries\n"
-    pkg = pkgs["libraries"][0]
-    out += f"* [{pkg.type}](https://github.com/ethz-asl/wavemap/blob/"
-    out += f"v{new_version_str}/{pkg.current_path}/CHANGELOG.rst)\n"
+    for pkg in pkgs["libraries"]:
+        out += f"* [{pkg.type}](https://github.com/ethz-asl/wavemap/blob/"
+        out += f"v{new_version_str}/{pkg.current_path}/CHANGELOG.rst)\n"
     out += "\n"
 
     out += "### Interfaces\n"
@@ -106,9 +113,13 @@ def draft_release_notes():
     out += "# Upgrade notes\n"
     out += "Upgrade instructions for\n"
     out += "* C++ Library\n"
-    out += "  * For instructions on setting up wavemap as a standalone CMake "
-    out += "project, please refer to [our docs]"
-    out += "(https://ethz-asl.github.io/wavemap/pages/installation/cmake)\n"
+    out += "  * To use wavemap as a standalone CMake project, please see "
+    out += "[these instructions]"
+    out += "(https://ethz-asl.github.io/wavemap/pages/installation/cpp)\n"
+    out += "* Python Library\n"
+    out += "  * To install wavemap's Python API, please see "
+    out += "[these instructions]"
+    out += "(https://ethz-asl.github.io/wavemap/pages/installation/python)\n"
     out += "* ROS1\n"
     out += "  * Catkin\n"
     out += "    * Go to your catkin workspace src directory: "
@@ -188,7 +199,7 @@ def prepare_release_files():
             print(f'Could NOT find changelog for {pkg_debug_name}')
             raise SystemExit
 
-        if pkg.type == PkgType.CPP:
+        if pkg.type in (PkgType.CPP, PkgType.PYTHON_BINDINGS):
             pkg_cmake_path = os.path.join(pkg.current_path, "CMakeLists.txt")
             if os.path.exists(pkg_cmake_path):
                 # Read the existing content of the CMakeLists.txt file
@@ -202,9 +213,9 @@ def prepare_release_files():
                 new_content, count = pattern.subn(substitution, cmake_content)
 
                 # Make the replacement was successful and unique
-                if count == 0:
-                    raise SystemExit
-                if 1 < count:
+                if count == 0 or 1 < count:
+                    print("Failed to update version number in "
+                          f"{pkg_cmake_path}")
                     raise SystemExit
 
                 # Write the updated content back to the file
@@ -215,6 +226,34 @@ def prepare_release_files():
                 print(f'Could NOT find CMakeLists.txt for {pkg_debug_name}')
                 raise SystemExit
 
+        if pkg.type == PkgType.PYTHON_BINDINGS:
+            pyproject_toml_path = os.path.join(pkg.current_path,
+                                               "pyproject.toml")
+            if os.path.exists(pyproject_toml_path):
+                # Read the existing content of the CMakeLists.txt file
+                with open(pyproject_toml_path, 'r', encoding='utf-8') as file:
+                    cmake_content = file.read()
+
+                # Replace the old version number with the new version number
+                pattern = re.compile(
+                    r'(version\s+=\s+)(\"\d+\.\d+\.\d+\")(.*)')
+                substitution = r'\1"' + new_version_str + r'"\3'
+                new_content, count = pattern.subn(substitution, cmake_content)
+
+                # Make the replacement was successful and unique
+                if count == 0 or 1 < count:
+                    print("Failed to update version number in "
+                          f"{pyproject_toml_path}")
+                    raise SystemExit
+
+                # Write the updated content back to the file
+                with open(pyproject_toml_path, 'w', encoding='utf-8') as file:
+                    file.write(new_content)
+
+            else:
+                print(f'Could NOT find CMakeLists.txt for {pkg_debug_name}')
+                raise SystemExit
+
         if pkg.type == PkgType.ROS1:
             pkg_xml_path = os.path.join(pkg.current_path, "package.xml")
             if os.path.exists(pkg_xml_path):
@@ -244,7 +283,10 @@ def prepare_release_files():
 
 # Parameters
 pkgs = {
-    "libraries": [Pkg('wavemap', PkgType.CPP, 'library/cpp', [])],
+    "libraries": [
+        Pkg('wavemap', PkgType.CPP, 'library/cpp', []),
+        Pkg('pywavemap', PkgType.PYTHON_BINDINGS, 'library/python', [])
+    ],
     "interfaces": [
         Pkg('wavemap', PkgType.ROS1, 'interfaces/ros1/wavemap', []),
         Pkg('wavemap_msgs', PkgType.ROS1, 'interfaces/ros1/wavemap_msgs', []),
@@ -260,7 +302,8 @@ def prepare_release_files():
     ],
     "examples": [
         Pkg('wavemap_examples_cpp', PkgType.CPP, 'examples/cpp', []),
-        Pkg('wavemap_examples_ros1', PkgType.ROS1, 'examples/ros1', [])
+        Pkg('wavemap_examples_ros1', PkgType.ROS1, 'examples/ros1', []),
+        Pkg('wavemap_examples_python', PkgType.PYTHON, 'examples/python', [])
     ]
 }
 

From d4b5dcb0c18ede1bcb3a9ba8960d1fada67a2570 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 18:14:37 +0200
Subject: [PATCH 83/85] Add GH Release and documentation site deployment back
 to GH workflows

---
 .github/workflows/docs.yml | 46 ++++++++++++++++++++++++++++++++++++--
 1 file changed, 44 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml
index c8e1763e1..93d6ab5c7 100644
--- a/.github/workflows/docs.yml
+++ b/.github/workflows/docs.yml
@@ -52,7 +52,7 @@ jobs:
             .
 
       - name: Upload tarball as GH Pages artifact
-        uses: actions/upload-artifact@main
+        uses: actions/upload-artifact@v3.2.1
         with:
           name: github-pages
           path: ${{github.workspace}}/docs/artifact.tar
@@ -64,8 +64,50 @@ jobs:
         run: sphinx-build -M latexpdf . _build/latex
 
       - name: Upload PDF
-        uses: actions/upload-artifact@main
+        uses: actions/upload-artifact@v3.2.1
         with:
           name: documentation-pdf
           path: ${{github.workspace}}/docs/_build/latex/latex/wavemap.pdf
           retention-days: 3
+
+  draft-release:
+    name: Draft Release
+    if: startsWith(github.event.ref, 'refs/tags/v')
+    needs: [ build-docs ]
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v4
+
+      - uses: actions/download-artifact@v4.1.8
+        with:
+          name: documentation-pdf
+
+      - name: Create Release
+        id: create_release
+        uses: softprops/action-gh-release@v2.0.8
+        with:
+          files: "wavemap.pdf"
+
+  publish-docs:
+    name: Publish docs
+    if: github.ref == 'refs/heads/main'
+    needs: [ build-docs ]
+    runs-on: ubuntu-20.04
+    permissions:
+      contents: read
+      pages: write
+      id-token: write
+    concurrency:
+      group: "pages"
+      cancel-in-progress: true
+    environment:
+      name: github-pages
+      url: ${{ steps.deployment.outputs.page_url }}
+    steps:
+      - name: Setup Pages
+        uses: actions/configure-pages@v4
+
+      - name: Deploy uploaded docs to GitHub Pages
+        id: deployment
+        uses: actions/deploy-pages@v3

From e12165e27b5aae7edfd845a497d285affdf5b88a Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Mon, 16 Sep 2024 18:28:59 +0200
Subject: [PATCH 84/85] Add Docker image deployment back to GH workflows

---
 .github/workflows/docs.yml |  6 +++---
 .github/workflows/ros1.yml | 30 +++++++++++++++++++++++++++++-
 2 files changed, 32 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml
index 93d6ab5c7..69766ff90 100644
--- a/.github/workflows/docs.yml
+++ b/.github/workflows/docs.yml
@@ -52,7 +52,7 @@ jobs:
             .
 
       - name: Upload tarball as GH Pages artifact
-        uses: actions/upload-artifact@v3.2.1
+        uses: actions/upload-artifact@v4.4.0
         with:
           name: github-pages
           path: ${{github.workspace}}/docs/artifact.tar
@@ -64,7 +64,7 @@ jobs:
         run: sphinx-build -M latexpdf . _build/latex
 
       - name: Upload PDF
-        uses: actions/upload-artifact@v3.2.1
+        uses: actions/upload-artifact@v4.4.0
         with:
           name: documentation-pdf
           path: ${{github.workspace}}/docs/_build/latex/latex/wavemap.pdf
@@ -90,7 +90,7 @@ jobs:
           files: "wavemap.pdf"
 
   publish-docs:
-    name: Publish docs
+    name: Publish to GH Pages
     if: github.ref == 'refs/heads/main'
     needs: [ build-docs ]
     runs-on: ubuntu-20.04
diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml
index 67bb300bb..30bcf2706 100644
--- a/.github/workflows/ros1.yml
+++ b/.github/workflows/ros1.yml
@@ -7,6 +7,7 @@ on:
 env:
   DOCKER_REGISTRY: ghcr.io
   DOCKER_CI_IMAGE_NAME: ci_wavemap_ros1
+  DOCKER_RELEASE_IMAGE_NAME: wavemap_ros1
   USER_HOME: /home/ci
   CATKIN_WS_PATH: /home/ci/catkin_ws
   CCACHE_DIR: /home/ci/ccache
@@ -62,7 +63,7 @@ jobs:
       - name: Test the image
         run: docker run --rm ${{ steps.ref-names.outputs.ci_image }}
 
-      - name: Push the image
+      - name: Push the CI image
         uses: docker/build-push-action@v6
         env:
           DOCKER_BUILD_SUMMARY: false
@@ -80,6 +81,33 @@ jobs:
           cache-from: type=registry,ref=${{ steps.ref-names.outputs.cache }}
           tags: ${{ steps.ref-names.outputs.ci_image }}
 
+      - name: Generate release image metadata
+        if: startsWith(github.event.ref, 'refs/tags/v')
+        id: meta
+        uses: docker/metadata-action@9ec57ed1fcdbf14dcef7dfbe97b2010124a938b7
+        with:
+          images: ${{ env.DOCKER_REGISTRY }}/${{ env.DOCKER_RELEASE_IMAGE_NAME }}
+
+      - name: Publish the release image
+        if: startsWith(github.event.ref, 'refs/tags/v')
+        uses: docker/build-push-action@v6
+        env:
+          DOCKER_BUILD_SUMMARY: false
+          DOCKER_BUILD_RECORD_UPLOAD: false
+        with:
+          context: ${{ github.repository }}
+          file: ${{ github.repository }}/tooling/docker/ros1/full.Dockerfile
+          target: workspace
+          build-args: |
+            REPOSITORY_NAME=${{ github.repository }}
+            USER_HOME=${{ env.USER_HOME }}
+            CATKIN_WS_PATH=${{ env.CATKIN_WS_PATH }}
+            CCACHE_DIR=${{ env.CCACHE_DIR }}
+          push: true
+          cache-from: type=registry,ref=${{ steps.ref-names.outputs.cache }}
+          tags: ${{ steps.meta.outputs.tags }}
+          labels: ${{ steps.meta.outputs.labels }}
+
   build:
     name: Build
     needs: workspace-container

From 1487a97dffe61c942d1ef15f1c3bc26022d61be2 Mon Sep 17 00:00:00 2001
From: Victor Reijgwart <victorreijgwart@gmail.com>
Date: Tue, 17 Sep 2024 11:55:27 +0200
Subject: [PATCH 85/85] Update changelogs

---
 .github/workflows/cpp.yml                     |  2 +-
 .github/workflows/docs.yml                    |  4 +--
 examples/cpp/CHANGELOG.rst                    |  7 ++++
 examples/cpp/CMakeLists.txt                   |  2 +-
 examples/python/CHANGELOG.rst                 |  5 +++
 examples/ros1/CHANGELOG.rst                   |  3 ++
 examples/ros1/package.xml                     |  2 +-
 interfaces/ros1/wavemap/CHANGELOG.rst         |  3 ++
 interfaces/ros1/wavemap/package.xml           |  2 +-
 interfaces/ros1/wavemap_all/CHANGELOG.rst     |  3 ++
 interfaces/ros1/wavemap_all/package.xml       |  2 +-
 interfaces/ros1/wavemap_msgs/CHANGELOG.rst    |  3 ++
 interfaces/ros1/wavemap_msgs/package.xml      |  2 +-
 interfaces/ros1/wavemap_ros/CHANGELOG.rst     |  3 ++
 interfaces/ros1/wavemap_ros/package.xml       |  2 +-
 .../wavemap_ros_conversions/CHANGELOG.rst     |  3 ++
 .../ros1/wavemap_ros_conversions/package.xml  |  2 +-
 .../ros1/wavemap_rviz_plugin/CHANGELOG.rst    |  3 ++
 .../ros1/wavemap_rviz_plugin/package.xml      |  2 +-
 library/cpp/CHANGELOG.rst                     | 32 +++++++++++++++++++
 library/cpp/CMakeLists.txt                    |  2 +-
 library/python/CHANGELOG.rst                  |  6 ++++
 library/python/CMakeLists.txt                 |  2 +-
 library/python/pyproject.toml                 |  2 +-
 tooling/packages/catkin_setup/CHANGELOG.rst   |  3 ++
 tooling/packages/catkin_setup/package.xml     |  2 +-
 26 files changed, 89 insertions(+), 15 deletions(-)

diff --git a/.github/workflows/cpp.yml b/.github/workflows/cpp.yml
index 713aeaeaa..fa8896558 100644
--- a/.github/workflows/cpp.yml
+++ b/.github/workflows/cpp.yml
@@ -91,7 +91,7 @@ jobs:
 
   clang-tidy:
     name: Clang tidy
-    needs: [ build ]
+    needs: build
     runs-on: ubuntu-20.04
     steps:
       - name: Fetch the package's repository
diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml
index 69766ff90..469045bf7 100644
--- a/.github/workflows/docs.yml
+++ b/.github/workflows/docs.yml
@@ -73,7 +73,7 @@ jobs:
   draft-release:
     name: Draft Release
     if: startsWith(github.event.ref, 'refs/tags/v')
-    needs: [ build-docs ]
+    needs: build-docs
     runs-on: ubuntu-20.04
     steps:
       - name: Checkout code
@@ -92,7 +92,7 @@ jobs:
   publish-docs:
     name: Publish to GH Pages
     if: github.ref == 'refs/heads/main'
-    needs: [ build-docs ]
+    needs: build-docs
     runs-on: ubuntu-20.04
     permissions:
       contents: read
diff --git a/examples/cpp/CHANGELOG.rst b/examples/cpp/CHANGELOG.rst
index b7a546dee..24e671576 100644
--- a/examples/cpp/CHANGELOG.rst
+++ b/examples/cpp/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package wavemap_examples_cpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+* Extend and improve documentation and examples
+* Minor changes to the C++ API for better consistency with the Python API
+* Extend map interpolation utils
+* Contributors: Victor Reijgwart
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/examples/cpp/CMakeLists.txt b/examples/cpp/CMakeLists.txt
index 2ab78e3d4..dbcad1701 100644
--- a/examples/cpp/CMakeLists.txt
+++ b/examples/cpp/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10)
-project(wavemap_examples_cpp VERSION 2.0.1 LANGUAGES CXX)
+project(wavemap_examples_cpp VERSION 2.1.0 LANGUAGES CXX)
 
 # Load the wavemap library
 # First, try to load it from sources
diff --git a/examples/python/CHANGELOG.rst b/examples/python/CHANGELOG.rst
index f9b6608ea..62250f1a3 100644
--- a/examples/python/CHANGELOG.rst
+++ b/examples/python/CHANGELOG.rst
@@ -1,3 +1,8 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package wavemap_examples_python
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.1.0 (2024-09-16)
+------------------
+* Initial examples and documentation on how to use wavemap's Python API
+* Contributors: Victor Reijgwart
diff --git a/examples/ros1/CHANGELOG.rst b/examples/ros1/CHANGELOG.rst
index 49a86df74..8a2f9c800 100644
--- a/examples/ros1/CHANGELOG.rst
+++ b/examples/ros1/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_examples_ros1
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/examples/ros1/package.xml b/examples/ros1/package.xml
index 8e76a6b48..0f43c187a 100644
--- a/examples/ros1/package.xml
+++ b/examples/ros1/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
   <name>wavemap_examples_ros1</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Usages examples for wavemap's ROS1 interface.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/interfaces/ros1/wavemap/CHANGELOG.rst b/interfaces/ros1/wavemap/CHANGELOG.rst
index b6bcf515f..3f375105f 100644
--- a/interfaces/ros1/wavemap/CHANGELOG.rst
+++ b/interfaces/ros1/wavemap/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/interfaces/ros1/wavemap/package.xml b/interfaces/ros1/wavemap/package.xml
index a588047bb..aacf26d7f 100644
--- a/interfaces/ros1/wavemap/package.xml
+++ b/interfaces/ros1/wavemap/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
   <name>wavemap</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Base library for wavemap.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/interfaces/ros1/wavemap_all/CHANGELOG.rst b/interfaces/ros1/wavemap_all/CHANGELOG.rst
index efe351a9d..fdf230e6a 100644
--- a/interfaces/ros1/wavemap_all/CHANGELOG.rst
+++ b/interfaces/ros1/wavemap_all/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_all
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/interfaces/ros1/wavemap_all/package.xml b/interfaces/ros1/wavemap_all/package.xml
index 470a1744c..233c9d405 100644
--- a/interfaces/ros1/wavemap_all/package.xml
+++ b/interfaces/ros1/wavemap_all/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
   <name>wavemap_all</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Metapackage that builds all wavemap packages.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/interfaces/ros1/wavemap_msgs/CHANGELOG.rst b/interfaces/ros1/wavemap_msgs/CHANGELOG.rst
index e5fa70504..6ed180bb8 100644
--- a/interfaces/ros1/wavemap_msgs/CHANGELOG.rst
+++ b/interfaces/ros1/wavemap_msgs/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/interfaces/ros1/wavemap_msgs/package.xml b/interfaces/ros1/wavemap_msgs/package.xml
index 5afc4f088..3bb0b6205 100644
--- a/interfaces/ros1/wavemap_msgs/package.xml
+++ b/interfaces/ros1/wavemap_msgs/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
   <name>wavemap_msgs</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Message definitions for wavemap's ROS interfaces.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/interfaces/ros1/wavemap_ros/CHANGELOG.rst b/interfaces/ros1/wavemap_ros/CHANGELOG.rst
index 477dd5a1d..b64bd5c76 100644
--- a/interfaces/ros1/wavemap_ros/CHANGELOG.rst
+++ b/interfaces/ros1/wavemap_ros/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 * Fix outdated Livox callback code
diff --git a/interfaces/ros1/wavemap_ros/package.xml b/interfaces/ros1/wavemap_ros/package.xml
index 924616f5d..5f2d4143e 100644
--- a/interfaces/ros1/wavemap_ros/package.xml
+++ b/interfaces/ros1/wavemap_ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
   <name>wavemap_ros</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>ROS interface for wavemap.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst b/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst
index a20337f58..5f6d01762 100644
--- a/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst
+++ b/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_ros_conversions
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/interfaces/ros1/wavemap_ros_conversions/package.xml b/interfaces/ros1/wavemap_ros_conversions/package.xml
index 37f9d27c5..4aa81f36f 100644
--- a/interfaces/ros1/wavemap_ros_conversions/package.xml
+++ b/interfaces/ros1/wavemap_ros_conversions/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
   <name>wavemap_ros_conversions</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Conversions between wavemap and ROS types.</description>
 
   <maintainer email="victorr@ethz.ch">Victor Reijgwart</maintainer>
diff --git a/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst b/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst
index 5f7b8887a..7c75939f9 100644
--- a/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst
+++ b/interfaces/ros1/wavemap_rviz_plugin/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_rviz_plugin
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/interfaces/ros1/wavemap_rviz_plugin/package.xml b/interfaces/ros1/wavemap_rviz_plugin/package.xml
index 568f05ff1..81ac381ff 100644
--- a/interfaces/ros1/wavemap_rviz_plugin/package.xml
+++ b/interfaces/ros1/wavemap_rviz_plugin/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
   <name>wavemap_rviz_plugin</name>
-  <version>2.0.1</version>
+  <version>2.1.0</version>
   <description>Plugin to interactively visualize maps published in wavemap's
     native format.
   </description>
diff --git a/library/cpp/CHANGELOG.rst b/library/cpp/CHANGELOG.rst
index 359198578..390759893 100644
--- a/library/cpp/CHANGELOG.rst
+++ b/library/cpp/CHANGELOG.rst
@@ -2,6 +2,38 @@
 Changelog for package wavemap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+* Improvements
+
+  * CMake
+
+    * Add CMake options to support embedding the C++ library in a Python pip pkg
+    * Improve auto-fetching of glog, switch to version with better CMake support
+
+  * C++
+
+    * Extend map interpolation utils
+    * Improve consistency between chunked and regular octree map interfaces
+    * Improve consistency between Pointcloud and Image data structures
+    * Add method to parse TypeSelector types directly from std::strings
+
+  * Documentation
+
+    * Improve C++ library installation instructions
+    * Improve and extend C++ library usage tutorial
+    * Add doxygen annotations for more C++ API classes and methods
+
+* Bug fixes
+
+  * Warn user and ignore range images of wrong dimensions to avoid segfaults
+  * Avoid out of bounds access bug in Haar coefficients print method
+  * Remove usage of deprecated STL types (avoid warnings from new GCC versions)
+  * Explicitly forbid shallow copying of wavemap maps to avoid nanobind errors
+  * Set glog logging level directly, not with gflags lib (might be unavailable)
+
+* Contributors: Victor Reijgwart
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt
index b4a50fba4..e61b3f5d8 100644
--- a/library/cpp/CMakeLists.txt
+++ b/library/cpp/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10)
-project(wavemap VERSION 2.0.1 LANGUAGES CXX)
+project(wavemap VERSION 2.1.0 LANGUAGES CXX)
 
 # General options
 cmake_policy(SET CMP0077 NEW)
diff --git a/library/python/CHANGELOG.rst b/library/python/CHANGELOG.rst
index 3e9303886..91a2ebb3d 100644
--- a/library/python/CHANGELOG.rst
+++ b/library/python/CHANGELOG.rst
@@ -1,3 +1,9 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 Changelog for package pywavemap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.1.0 (2024-09-16)
+------------------
+* First version of wavemap's Python API
+* Including tests and documentation on how to install and use it
+* Contributors: Victor Reijgwart
diff --git a/library/python/CMakeLists.txt b/library/python/CMakeLists.txt
index 074d9d053..23ad9ab77 100644
--- a/library/python/CMakeLists.txt
+++ b/library/python/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.18)
-project(pywavemap VERSION 2.0.1 LANGUAGES CXX)
+project(pywavemap VERSION 2.1.0 LANGUAGES CXX)
 
 # Warn if the user invokes CMake directly
 if (NOT SKBUILD AND NOT $ENV{CLION_IDE})
diff --git a/library/python/pyproject.toml b/library/python/pyproject.toml
index 72ec86443..707601fe6 100644
--- a/library/python/pyproject.toml
+++ b/library/python/pyproject.toml
@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
 
 [project]
 name = "pywavemap"
-version = "2.0.0"
+version = "2.1.0"
 description = "A fast, efficient and accurate multi-resolution, multi-sensor 3D occupancy mapping framework."
 readme = "../../README.md"
 requires-python = ">=3.8"
diff --git a/tooling/packages/catkin_setup/CHANGELOG.rst b/tooling/packages/catkin_setup/CHANGELOG.rst
index 000c72387..68e8049a9 100644
--- a/tooling/packages/catkin_setup/CHANGELOG.rst
+++ b/tooling/packages/catkin_setup/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package catkin_setup
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.1.0 (2024-09-16)
+------------------
+
 2.0.1 (2024-08-30)
 ------------------
 
diff --git a/tooling/packages/catkin_setup/package.xml b/tooling/packages/catkin_setup/package.xml
index 9768b3643..38c566b28 100644
--- a/tooling/packages/catkin_setup/package.xml
+++ b/tooling/packages/catkin_setup/package.xml
@@ -1,7 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <package format="2">
     <name>catkin_setup</name>
-    <version>2.0.1</version>
+    <version>2.1.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>