ethz-asl · victorreijgwart · Nov 27, 2024 · Nov 8, 2024 · Nov 8, 2024 · Nov 9, 2024
diff --git a/docs/pages/tutorials/python.rst b/docs/pages/tutorials/python.rst
@@ -104,10 +104,6 @@ If you need to look up multiple node values, we recommend using our batched quer
 .. 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.
-
 .. _python-code-examples-interpolation:
 
 Real coordinates

diff --git a/interfaces/ros1/wavemap/CMakeLists.txt b/interfaces/ros1/wavemap/CMakeLists.txt
@@ -27,8 +27,8 @@ add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp
 
 # Include the library's tests in catkin's run_tests target
 include(cmake/append_to_catkin_tests.cmake)
-append_to_catkin_tests(test_wavemap_io)
 append_to_catkin_tests(test_wavemap_core)
+append_to_catkin_tests(test_wavemap_io)
 
 # Support installs
 # Mark targets for installation
@@ -38,6 +38,6 @@ install(TARGETS wavemap_core wavemap_io wavemap_pipeline
     RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 # Mark headers for installation
 install(DIRECTORY
-    ${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp/include/wavemap/
+    "${CMAKE_CURRENT_SOURCE_DIR}/../../../library/cpp/include/wavemap/"
     DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
     FILES_MATCHING PATTERN "*.h")
diff --git a/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h b/interfaces/ros1/wavemap_ros/include/wavemap_ros/utils/tf_transformer.h
@@ -2,6 +2,7 @@
 #define WAVEMAP_ROS_UTILS_TF_TRANSFORMER_H_
 
 #include <map>
+#include <optional>
 #include <string>
 
 #include <tf2_ros/transform_listener.h>
@@ -26,13 +27,11 @@ class TfTransformer {
                         const ros::Time& frame_timestamp);
 
   // Lookup transforms and convert them to Kindr
-  bool lookupTransform(const std::string& to_frame_id,
-                       const std::string& from_frame_id,
-                       const ros::Time& frame_timestamp,
-                       Transformation3D& transform);
-  bool lookupLatestTransform(const std::string& to_frame_id,
-                             const std::string& from_frame_id,
-                             Transformation3D& transform);
+  std::optional<Transformation3D> lookupTransform(
+      const std::string& to_frame_id, const std::string& from_frame_id,
+      const ros::Time& frame_timestamp);
+  std::optional<Transformation3D> lookupLatestTransform(
+      const std::string& to_frame_id, const std::string& from_frame_id);
 
   // Strip leading slashes if needed to avoid TF errors
   static std::string sanitizeFrameId(const std::string& string);
@@ -50,10 +49,9 @@ class TfTransformer {
   bool waitForTransformImpl(const std::string& to_frame_id,
                             const std::string& from_frame_id,
                             const ros::Time& frame_timestamp) const;
-  bool lookupTransformImpl(const std::string& to_frame_id,
-                           const std::string& from_frame_id,
-                           const ros::Time& frame_timestamp,
-                           Transformation3D& transform);
+  std::optional<Transformation3D> lookupTransformImpl(
+      const std::string& to_frame_id, const std::string& from_frame_id,
+      const ros::Time& frame_timestamp);
 };
 }  // namespace wavemap
 

diff --git a/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/depth_image_topic_input.cc
@@ -71,11 +71,11 @@ void DepthImageTopicInput::processQueue() {
                                             : config_.sensor_frame_id;
 
     // Get the sensor pose in world frame
-    Transformation3D T_W_C;
     const ros::Time stamp =
         oldest_msg.header.stamp + ros::Duration(config_.time_offset);
-    if (!transformer_->lookupTransform(world_frame_, sensor_frame_id, stamp,
-                                       T_W_C)) {
+    const auto T_W_C =
+        transformer_->lookupTransform(world_frame_, sensor_frame_id, stamp);
+    if (!T_W_C) {
       const auto newest_msg = depth_image_queue_.back();
       if ((newest_msg.header.stamp - oldest_msg.header.stamp).toSec() <
           config_.max_wait_for_pose) {
@@ -105,7 +105,7 @@ void DepthImageTopicInput::processQueue() {
     // Create the posed depth image input
     PosedImage<> posed_depth_image(cv_image->image.rows, cv_image->image.cols);
     cv::cv2eigen<FloatingPoint>(cv_image->image, posed_depth_image.getData());
-    posed_depth_image.setPose(T_W_C);
+    posed_depth_image.setPose(*T_W_C);
 
     // Integrate the depth image
     ROS_DEBUG_STREAM("Inserting depth image with "

diff --git a/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc b/interfaces/ros1/wavemap_ros/src/inputs/pointcloud_topic_input.cc
@@ -236,16 +236,16 @@ void PointcloudTopicInput::processQueue() {
       }
     } else {
       // Get the pointcloud's pose
-      Transformation3D T_W_C;
-      if (!transformer_->lookupTransform(
-              world_frame_, oldest_msg.getSensorFrame(),
-              convert::nanoSecondsToRosTime(oldest_msg.getTimeBase()), T_W_C)) {
+      const auto T_W_C = transformer_->lookupTransform(
+          world_frame_, oldest_msg.getSensorFrame(),
+          convert::nanoSecondsToRosTime(oldest_msg.getTimeBase()));
+      if (!T_W_C) {
         // Try to get this pointcloud's pose again at the next iteration
         return;
       }
 
       // Convert to a posed pointcloud
-      posed_pointcloud = PosedPointcloud<>(T_W_C);
+      posed_pointcloud = PosedPointcloud<>(*T_W_C);
       posed_pointcloud.resize(oldest_msg.getPoints().size());
       for (unsigned point_idx = 0; point_idx < oldest_msg.getPoints().size();
            ++point_idx) {

diff --git a/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc b/interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc
@@ -49,9 +49,9 @@ void CropMapOperation::run(bool force_run) {
     return;
   }
 
-  Transformation3D T_W_B;
-  if (!transformer_->lookupTransform(world_frame_, config_.body_frame,
-                                     current_time, T_W_B)) {
+  const auto T_W_B = transformer_->lookupTransform(
+      world_frame_, config_.body_frame, current_time);
+  if (!T_W_B) {
     ROS_WARN_STREAM(
         "Could not look up center point for map cropping. TF lookup of "
         "body_frame \""
@@ -62,7 +62,7 @@ void CropMapOperation::run(bool force_run) {
 
   const IndexElement tree_height = occupancy_map_->getTreeHeight();
   const FloatingPoint min_cell_width = occupancy_map_->getMinCellWidth();
-  const Point3D t_W_B = T_W_B.getPosition();
+  const Point3D t_W_B = T_W_B->getPosition();
 
   auto indicator_fn = [tree_height, min_cell_width, &config = config_, &t_W_B](
                           const Index3D& block_index, const auto& /*block*/) {

diff --git a/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc b/interfaces/ros1/wavemap_ros/src/utils/pointcloud_undistorter.cc
@@ -42,15 +42,15 @@ PointcloudUndistorter::Result PointcloudUndistorter::undistortPointcloud(
   undistortion::StampedPoseBuffer pose_buffer;
   pose_buffer.reserve(num_time_steps);
   for (int step_idx = 0; step_idx < num_time_steps; ++step_idx) {
-    auto& stamped_pose = pose_buffer.emplace_back();
-    stamped_pose.stamp = start_time + step_idx * step_size;
-    if (!transformer_->lookupTransform(
-            fixed_frame, stamped_pointcloud.getSensorFrame(),
-            convert::nanoSecondsToRosTime(stamped_pose.stamp),
-            stamped_pose.pose)) {
+    const auto stamp = start_time + step_idx * step_size;
+    const auto pose = transformer_->lookupTransform(
+        fixed_frame, stamped_pointcloud.getSensorFrame(),
+        convert::nanoSecondsToRosTime(stamp));
+    if (pose) {
+      pose_buffer.emplace_back(stamp, *pose);
+    } else {
       ROS_WARN_STREAM("Failed to buffer intermediate pose at time "
-                      << convert::nanoSecondsToRosTime(stamped_pose.stamp)
-                      << ".");
+                      << convert::nanoSecondsToRosTime(stamp) << ".");
       return Result::kIntermediateTimeNotInTfBuffer;
     }
   }

diff --git a/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc b/interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc
@@ -20,20 +20,17 @@ bool TfTransformer::waitForTransform(const std::string& to_frame_id,
                               sanitizeFrameId(from_frame_id), frame_timestamp);
 }
 
-bool TfTransformer::lookupLatestTransform(const std::string& to_frame_id,
-                                          const std::string& from_frame_id,
-                                          Transformation3D& transform) {
+std::optional<Transformation3D> TfTransformer::lookupLatestTransform(
+    const std::string& to_frame_id, const std::string& from_frame_id) {
   return lookupTransformImpl(sanitizeFrameId(to_frame_id),
-                             sanitizeFrameId(from_frame_id), {}, transform);
+                             sanitizeFrameId(from_frame_id), {});
 }
 
-bool TfTransformer::lookupTransform(const std::string& to_frame_id,
-                                    const std::string& from_frame_id,
-                                    const ros::Time& frame_timestamp,
-                                    Transformation3D& transform) {
+std::optional<Transformation3D> TfTransformer::lookupTransform(
+    const std::string& to_frame_id, const std::string& from_frame_id,
+    const ros::Time& frame_timestamp) {
   return lookupTransformImpl(sanitizeFrameId(to_frame_id),
-                             sanitizeFrameId(from_frame_id), frame_timestamp,
-                             transform);
+                             sanitizeFrameId(from_frame_id), frame_timestamp);
 }
 
 std::string TfTransformer::sanitizeFrameId(const std::string& string) {
@@ -64,16 +61,14 @@ bool TfTransformer::waitForTransformImpl(
   return false;
 }
 
-bool TfTransformer::lookupTransformImpl(const std::string& to_frame_id,
-                                        const std::string& from_frame_id,
-                                        const ros::Time& frame_timestamp,
-                                        Transformation3D& transform) {
+std::optional<Transformation3D> TfTransformer::lookupTransformImpl(
+    const std::string& to_frame_id, const std::string& from_frame_id,
+    const ros::Time& frame_timestamp) {
   if (!isTransformAvailable(to_frame_id, from_frame_id, frame_timestamp)) {
-    return false;
+    return std::nullopt;
   }
   geometry_msgs::TransformStamped transform_msg =
       tf_buffer_.lookupTransform(to_frame_id, from_frame_id, frame_timestamp);
-  transform = convert::transformMsgToTransformation3D(transform_msg.transform);
-  return true;
+  return convert::transformMsgToTransformation3D(transform_msg.transform);
 }
 }  // namespace wavemap
diff --git a/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc b/interfaces/ros1/wavemap_ros_conversions/src/map_msg_conversions.cc
@@ -335,7 +335,7 @@ void blockToRosMsg(const HashedWaveletOctree::BlockIndex& block_index,
   // Convenience type for elements on the stack used to iterate over the map
   struct StackElement {
     const FloatingPoint scale;
-    const HashedWaveletOctreeBlock::NodeType& node;
+    HashedWaveletOctreeBlock::OctreeType::NodeConstRefType node;
   };
 
   // Serialize the block's metadata
@@ -534,7 +534,7 @@ void blockToRosMsg(const HashedChunkedWaveletOctree::BlockIndex& block_index,
   // Define convenience types and constants
   struct StackElement {
     const FloatingPoint scale;
-    HashedChunkedWaveletOctreeBlock::ChunkedOctreeType::NodeConstRefType node;
+    HashedChunkedWaveletOctreeBlock::OctreeType::NodeConstRefType node;
   };
 
   // Serialize the block's metadata

diff --git a/library/cpp/CMakeLists.txt b/library/cpp/CMakeLists.txt
@@ -11,6 +11,7 @@ option(BUILD_SHARED_LIBS
 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)
+option(TRACY_ENABLE "Enable profiling with Tracy" OFF)
 
 # CMake helpers and general wavemap tooling (e.g. to run clang-tidy CI)
 include(GNUInstallDirs)

diff --git a/library/cpp/cmake/find-wavemap-deps.cmake b/library/cpp/cmake/find-wavemap-deps.cmake
@@ -55,4 +55,14 @@ else ()
 endif ()
 
 # Optional dependencies
-find_package(tracy QUIET)
+if (TRACY_ENABLE)
+  find_package(Tracy QUIET)
+  if (NOT Tracy_FOUND)
+    message(FATAL_ERROR
+        "Profiling is enabled (TRACY_ENABLE=ON), but Tracy is not available. "
+        "Please switch it to OFF or install Tracy as a system package with: "
+        "cmake -S . -B build -DTRACY_ENABLE=ON -DBUILD_SHARED_LIBS=true "
+        "-DCMAKE_BUILD_TYPE=Release && cmake --build build --parallel && "
+        "sudo make install")
+  endif ()
+endif ()
diff --git a/library/cpp/cmake/wavemap-extras.cmake b/library/cpp/cmake/wavemap-extras.cmake
@@ -38,6 +38,11 @@ function(set_wavemap_target_properties target)
   # General C++ defines
   target_compile_definitions(${target} PUBLIC EIGEN_INITIALIZE_MATRICES_BY_NAN)
 
+  # Profiling related C++ defines
+  if (TRACY_ENABLE)
+    target_compile_definitions(${target} PUBLIC TRACY_ENABLE)
+  endif ()
+
   # Conditional compilation options
   if (DCHECK_ALWAYS_ON)
     target_compile_definitions(${target} PUBLIC DCHECK_ALWAYS_ON)

diff --git a/library/cpp/include/wavemap/core/config/string_list.h b/library/cpp/include/wavemap/core/config/string_list.h
@@ -15,10 +15,15 @@ struct StringList {
 
   // Constructors
   StringList() = default;
-  StringList(ValueType value) : value(std::move(value)) {}  // NOLINT
+  StringList(const ValueType& value) : value(value) {}        // NOLINT
+  StringList(ValueType&& value) : value(std::move(value)) {}  // NOLINT
 
   // Assignment operator
-  StringList& operator=(ValueType rhs) {
+  StringList& operator=(const ValueType& rhs) {
+    value = rhs;
+    return *this;
+  }
+  StringList& operator=(ValueType&& rhs) {
     value = std::move(rhs);
     return *this;
   }
@@ -29,8 +34,8 @@ struct StringList {
   bool operator!=(const StringList& rhs) const { return value != rhs.value; }
 
   // Allow implicit conversions to the underlying type
-  operator ValueType&() { return value; }
-  operator const ValueType&() const { return value; }
+  operator ValueType&() { return value; }              // NOLINT
+  operator const ValueType&() const { return value; }  // NOLINT
 
   // Method to load from configs
   static std::optional<StringList> from(const param::Value& param);

diff --git a/library/cpp/include/wavemap/core/data_structure/aabb.h b/library/cpp/include/wavemap/core/data_structure/aabb.h
@@ -1,8 +1,10 @@
 #ifndef WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_
 #define WAVEMAP_CORE_DATA_STRUCTURE_AABB_H_
 
+#include <algorithm>
 #include <limits>
 #include <string>
+#include <utility>
 
 #include "wavemap/core/common.h"
 #include "wavemap/core/utils/math/int_math.h"
@@ -23,6 +25,10 @@ struct AABB {
   PointType min = PointType::Constant(kInitialMin);
   PointType max = PointType::Constant(kInitialMax);
 
+  AABB() = default;
+  AABB(const PointT& min, const PointT& max) : min(min), max(max) {}
+  AABB(PointT&& min, PointT&& max) : min(std::move(min)), max(std::move(max)) {}
+
   void includePoint(const PointType& point) {
     min = min.cwiseMin(point);
     max = max.cwiseMax(point);

diff --git a/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h b/library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree.h
@@ -28,20 +28,14 @@ class ChunkedNdtree {
   explicit ChunkedNdtree(HeightType max_height);
   ~ChunkedNdtree() = default;
 
-  bool empty() const { return root_chunk_.empty(); }
+  bool empty() const { return getRootNode().empty(); }
   size_t size() const;
   void clear() { root_chunk_.clear(); }
   void prune();
 
   HeightType getMaxHeight() const { return max_height_; }
   size_t getMemoryUsage() const;
 
-  // TODO(victorr): Add methods to directly query and operate on chunks,
-  //                once a proper index type for chunks has been defined.
-  //                The ChunkIndex type would be similar to NdtreeIndex, but has
-  //                to account for the chunks having a branching factor that
-  //                differs from 2 (probably 2^(dim * chunk_height)).
-
   bool hasNode(const IndexType& index) const { return getNode(index); }
   NodePtrType getNode(const IndexType& index);
   NodeConstPtrType getNode(const IndexType& index) const;