Merge pull request #84 from ethz-asl/feature/simplify_chunked_octree_…

…traversals Simplify chunked octree traversals
ethz-asl · Nov 27, 2024 · 557b37a · 557b37a
2 parents fa4b8e2 + 85320a0
commit 557b37a
Show file tree

Hide file tree

Showing 81 changed files with 996 additions and 815 deletions.
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/examples/cpp/CHANGELOG.rst b/examples/cpp/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_examples_cpp
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+
 2.1.2 (2024-11-20)
 ------------------
 

diff --git 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.1.2 LANGUAGES CXX)
+project(wavemap_examples_cpp VERSION 2.2.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
@@ -2,6 +2,9 @@
 Changelog for package wavemap_examples_python
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+
 2.1.2 (2024-11-20)
 ------------------
 

diff --git a/examples/ros1/CHANGELOG.rst b/examples/ros1/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_examples_ros1
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+
 2.1.2 (2024-11-20)
 ------------------
 

diff --git 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.1.2</version>
+  <version>2.2.0</version>
   <description>Usages examples for wavemap's ROS1 interface.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

diff --git a/interfaces/ros1/wavemap/CHANGELOG.rst b/interfaces/ros1/wavemap/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+
 2.1.2 (2024-11-20)
 ------------------
 

diff --git 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.1.2</version>
+  <version>2.2.0</version>
   <description>Base library for wavemap.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

diff --git a/interfaces/ros1/wavemap_all/CHANGELOG.rst b/interfaces/ros1/wavemap_all/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_all
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+
 2.1.2 (2024-11-20)
 ------------------
 

diff --git 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.1.2</version>
+  <version>2.2.0</version>
   <description>Metapackage that builds all wavemap packages.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

diff --git a/interfaces/ros1/wavemap_msgs/CHANGELOG.rst b/interfaces/ros1/wavemap_msgs/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package wavemap_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+
 2.1.2 (2024-11-20)
 ------------------
 

diff --git 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.1.2</version>
+  <version>2.2.0</version>
   <description>Message definitions for wavemap's ROS interfaces.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

diff --git a/interfaces/ros1/wavemap_ros/CHANGELOG.rst b/interfaces/ros1/wavemap_ros/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package wavemap_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+* Refactor TfTransformer to return by value, using std::nullopt to signal failure
+* Contributors: Victor Reijgwart
+
 2.1.2 (2024-11-20)
 ------------------
 * Report CPU, wall time and RAM usage when rosbag_processor completes

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/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.1.2</version>
+  <version>2.2.0</version>
   <description>ROS interface for wavemap.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

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/CHANGELOG.rst b/interfaces/ros1/wavemap_ros_conversions/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package wavemap_ros_conversions
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.2.0 (2024-11-25)
+------------------
+* Improve consistency between code operating on standard and chunked octrees
+* Contributors: Victor Reijgwart
+
 2.1.2 (2024-11-20)
 ------------------
 * Update include path for profiler_interface.h

diff --git 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.1.2</version>
+  <version>2.2.0</version>
   <description>Conversions between wavemap and ROS types.</description>
 
   <maintainer email="[email protected]">Victor Reijgwart</maintainer>

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/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.2.0 (2024-11-25)
+------------------
+
 2.1.2 (2024-11-20)
 ------------------
 * Report CPU, wall time and RAM usage when rosbag_processor completes

diff --git 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.1.2</version>
+  <version>2.2.0</version>
   <description>Plugin to interactively visualize maps published in wavemap's
     native format.
   </description>