Simplify chunked octree traversals

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

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
 

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 "

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) {

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*/) {

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;
     }
   }

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

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

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);

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);

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;

library/cpp/include/wavemap/core/data_structure/chunked_ndtree/chunked_ndtree_node_address.h

@@ -15,7 +15,6 @@ template <typename ChunkType>
 class ChunkedNdtreeNodePtr {
  public:
   using NodeRef = ChunkedNdtreeNodeRef<ChunkType>;
-  using NodeConstRef = ChunkedNdtreeNodeRef<const ChunkType>;
 
   // Constructors
   ChunkedNdtreeNodePtr() = default;
@@ -32,28 +31,29 @@ class ChunkedNdtreeNodePtr {
 
   // Emulate pointer semantics
   void reset() { node_.reset(); }
-  operator bool() const { return node_.has_value(); }  // NOLINT
-  NodeRef operator*() { return node_.value(); }
-  NodeConstRef operator*() const { return node_.value(); }
+  NodeRef operator*() const { return node_.value(); }
   NodeRef* operator->() { return node_.operator->(); }
-  const NodeConstRef* operator->() const { return node_.operator->(); }
+  const NodeRef* operator->() const { return node_.operator->(); }
+
+  // Emulate null check semantics
+  operator bool() const { return node_.has_value(); }  // NOLINT
+  bool operator==(std::nullptr_t) noexcept { return !node_.has_value(); }
 
  private:
-  std::optional<NodeRef> node_;
+  std::optional<NodeRef> node_{};
 };
 
 template <typename ChunkType>
 class ChunkedNdtreeNodeRef {
  public:
   using NodeRef = ChunkedNdtreeNodeRef<ChunkType>;
-  using NodeConstRef = ChunkedNdtreeNodeRef<const ChunkType>;
   using NodePtr = ChunkedNdtreeNodePtr<ChunkType>;
-  using NodeConstPtr = ChunkedNdtreeNodePtr<const ChunkType>;
 
   static constexpr int kDim = ChunkType::kDim;
   static constexpr int kNumChildren = NdtreeIndex<kDim>::kNumChildren;
   using NodeDataType = typename ChunkType::DataType;
 
+  ChunkedNdtreeNodeRef() = delete;
   ChunkedNdtreeNodeRef(ChunkType& chunk, IndexElement relative_node_depth,
                        LinearIndex level_traversal_distance);
 
@@ -65,35 +65,36 @@ class ChunkedNdtreeNodeRef {
   ChunkedNdtreeNodeRef& operator=(const ChunkedNdtreeNodeRef&) = delete;
   ChunkedNdtreeNodeRef& operator=(ChunkedNdtreeNodeRef&&) = delete;
 
-  // Conversion to const ref
-  operator NodeConstRef();
+  // Conversion of non-const ref to const ref
+  operator ChunkedNdtreeNodeRef<const ChunkType>() const;  // NOLINT
 
   // Conversion to pointer
-  NodePtr operator&();             // NOLINT
-  NodeConstPtr operator&() const;  // NOLINT
+  NodePtr operator&() const;  // NOLINT
 
   // Regular node methods
   bool empty() const;
 
   bool hasNonzeroData() const;
   bool hasNonzeroData(FloatingPoint threshold) const;
-  auto& data();
-  const auto& data() const;
+  auto& data() const;
 
-  typename ChunkType::BitRef hasAtLeastOneChild();
-  bool hasAtLeastOneChild() const;
+  auto hasAtLeastOneChild() const;  // Returns a BitRef or bool, depending on
+                                    // whether the ChunkType is const-qualified
 
   bool hasChild(NdtreeIndexRelativeChild child_index) const;
+  // TODO(victorr): Add tests for this method
+  void eraseChild(NdtreeIndexRelativeChild child_index) const;
 
-  NodePtr getChild(NdtreeIndexRelativeChild child_index);
-  NodeConstPtr getChild(NdtreeIndexRelativeChild child_index) const;
+  NodePtr getChild(NdtreeIndexRelativeChild child_index) const;
   template <typename... DefaultArgs>
   NodeRef getOrAllocateChild(NdtreeIndexRelativeChild child_index,
                              DefaultArgs&&... args) const;
 
  private:
   ChunkType& chunk_;
+  // TODO(victorr): Benchmark with int8_t
   const IndexElement relative_node_depth_ = 0;
+  // TODO(victorr): Benchmark with uint32_t
   const LinearIndex level_traversal_distance_ = 0u;
 
   LinearIndex computeRelativeNodeIndex() const;

library/cpp/include/wavemap/core/data_structure/chunked_ndtree/impl/chunked_ndtree_chunk_inl.h

@@ -15,7 +15,8 @@ bool ChunkedNdtreeChunk<DataT, dim, height>::empty() const {
 template <typename DataT, int dim, int height>
 void ChunkedNdtreeChunk<DataT, dim, height>::clear() {
   deleteChildrenArray();
-  std::fill(node_data_.begin(), node_data_.end(), DataT{});
+  node_data_.fill({});
+  node_has_at_least_one_child_.reset();
 }
 
 template <typename DataT, int dim, int height>