Crop map at voxel resolution

interfaces/ros1/wavemap_ros/include/wavemap_ros/map_operations/crop_map_operation.h

@@ -7,6 +7,7 @@
 
 #include <wavemap/core/config/config_base.h>
 #include <wavemap/core/map/map_base.h>
+#include <wavemap/core/utils/time/stopwatch.h>
 #include <wavemap/pipeline/map_operations/map_operation_base.h>
 
 #include "wavemap_ros/utils/tf_transformer.h"
@@ -15,17 +16,28 @@ namespace wavemap {
 /**
  * Config struct for map cropping operations.
  */
-struct CropMapOperationConfig : public ConfigBase<CropMapOperationConfig, 3> {
+struct CropMapOperationConfig : public ConfigBase<CropMapOperationConfig, 5> {
   //! Time period controlling how often the map is cropped.
   Seconds<FloatingPoint> once_every = 10.f;
 
   //! Name of the TF frame to treat as the center point. Usually the robot's
-  //! body frame. When the cropper runs, all blocks that are further than
-  //! remove_blocks_beyond_distance from this point are deleted.
+  //! body frame. When the cropper runs, all cells that are further than
+  //! `radius` from this point are deleted.
   std::string body_frame = "body";
 
-  //! Distance beyond which blocks are deleted when the cropper is executed.
-  Meters<FloatingPoint> remove_blocks_beyond_distance;
+  //! Time offset applied when retrieving the transform from body_frame to
+  //! world_frame. Set to -1 to use the most recent transform available in ROS
+  //! TF, ignoring timestamps (default). If set to a non-negative value, the
+  //! transform lookup uses a timestamp of `ros::Time::now() - tf_time_offset`.
+  Seconds<FloatingPoint> tf_time_offset = -1.f;
+
+  //! Distance beyond which to remove cells from the map.
+  Meters<FloatingPoint> radius;
+
+  //! Maximum resolution at which the crop is applied. Set to 0 to match the
+  //! map's maximum resolution (default). Setting a higher value reduces
+  //! computation but produces more jagged borders.
+  Meters<FloatingPoint> max_update_resolution = 0.f;
 
   static MemberMap memberMap;
 
@@ -48,6 +60,14 @@ class CropMapOperation : public MapOperationBase {
   const std::shared_ptr<TfTransformer> transformer_;
   const std::string world_frame_;
   ros::Time last_run_timestamp_;
+  Stopwatch timer_;
+
+  const FloatingPoint min_cell_width_ = occupancy_map_->getMinCellWidth();
+  const IndexElement termination_height_ =
+      min_cell_width_ < config_.max_update_resolution
+          ? static_cast<IndexElement>(std::round(
+                std::log2(config_.max_update_resolution / min_cell_width_)))
+          : 0;
 };
 }  // namespace wavemap
 

interfaces/ros1/wavemap_ros/src/map_operations/crop_map_operation.cc

@@ -7,19 +7,25 @@
 #include <wavemap/core/map/hashed_blocks.h>
 #include <wavemap/core/map/hashed_chunked_wavelet_octree.h>
 #include <wavemap/core/map/hashed_wavelet_octree.h>
+#include <wavemap/core/utils/edit/crop.h>
 
 namespace wavemap {
 DECLARE_CONFIG_MEMBERS(CropMapOperationConfig,
                       (once_every)
                       (body_frame)
-                      (remove_blocks_beyond_distance));
+                      (tf_time_offset)
+                      (radius)
+                      (max_update_resolution));
 
 bool CropMapOperationConfig::isValid(bool verbose) const {
   bool all_valid = true;
 
   all_valid &= IS_PARAM_GT(once_every, 0.f, verbose);
   all_valid &= IS_PARAM_NE(body_frame, "", verbose);
-  all_valid &= IS_PARAM_GT(remove_blocks_beyond_distance, 0.f, verbose);
+  all_valid &=
+      IS_PARAM_TRUE(tf_time_offset == -1.f || 0.f <= tf_time_offset, verbose);
+  all_valid &= IS_PARAM_GT(radius, 0.f, verbose);
+  all_valid &= IS_PARAM_GE(max_update_resolution, 0.f, verbose);
 
   return all_valid;
 }
@@ -49,42 +55,49 @@ void CropMapOperation::run(bool force_run) {
     return;
   }
 
+  const bool use_most_recent_transform = config_.tf_time_offset < 0.f;
+  const ros::Time timestamp =
+      use_most_recent_transform
+          ? ros::Time::UNINITIALIZED
+          : current_time - ros::Duration(config_.tf_time_offset);
   const auto T_W_B = transformer_->lookupTransform(
-      world_frame_, config_.body_frame, current_time);
+      world_frame_, config_.body_frame, timestamp);
   if (!T_W_B) {
-    ROS_WARN_STREAM(
-        "Could not look up center point for map cropping. TF lookup of "
-        "body_frame \""
-        << config_.body_frame << "\" w.r.t. world_frame \"" << world_frame_
-        << "\" at time " << current_time << " failed.");
+    if (use_most_recent_transform) {
+      ROS_WARN_STREAM(
+          "Could not look up center point for map cropping. No TF from "
+          "body_frame \""
+          << config_.body_frame << "\" to world_frame \"" << world_frame_
+          << "\".");
+    } else {
+      ROS_WARN_STREAM(
+          "Could not look up center point for map cropping. TF lookup from "
+          "body_frame \""
+          << config_.body_frame << "\" to world_frame \"" << world_frame_
+          << "\" at time " << timestamp << " failed.");
+    }
     return;
   }
 
-  const IndexElement tree_height = occupancy_map_->getTreeHeight();
-  const FloatingPoint min_cell_width = occupancy_map_->getMinCellWidth();
-  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*/) {
-    const auto block_node_index = OctreeIndex{tree_height, block_index};
-    const auto block_aabb =
-        convert::nodeIndexToAABB(block_node_index, min_cell_width);
-    const FloatingPoint d_B_block = block_aabb.minDistanceTo(t_W_B);
-    return config.remove_blocks_beyond_distance < d_B_block;
-  };
-
+  timer_.start();
   if (auto* hashed_wavelet_octree =
           dynamic_cast<HashedWaveletOctree*>(occupancy_map_.get());
       hashed_wavelet_octree) {
-    hashed_wavelet_octree->eraseBlockIf(indicator_fn);
+    crop_to_sphere(T_W_B->getPosition(), config_.radius, *hashed_wavelet_octree,
+                   termination_height_);
   } else if (auto* hashed_chunked_wavelet_octree =
                  dynamic_cast<HashedChunkedWaveletOctree*>(
                      occupancy_map_.get());
              hashed_chunked_wavelet_octree) {
-    hashed_chunked_wavelet_octree->eraseBlockIf(indicator_fn);
+    crop_to_sphere(T_W_B->getPosition(), config_.radius,
+                   *hashed_chunked_wavelet_octree, termination_height_);
   } else {
     ROS_WARN(
         "Map cropping is only supported for hash-based map data structures.");
   }
+  timer_.stop();
+  ROS_DEBUG_STREAM("Cropped map in " << timer_.getLastEpisodeDuration()
+                                     << "s. Total cropping time: "
+                                     << timer_.getTotalDuration() << "s.");
 }
 }  // namespace wavemap

interfaces/ros1/wavemap_ros/src/utils/tf_transformer.cc

@@ -23,7 +23,8 @@ bool TfTransformer::waitForTransform(const std::string& to_frame_id,
 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), {});
+                             sanitizeFrameId(from_frame_id),
+                             ros::Time::UNINITIALIZED);
 }
 
 std::optional<Transformation3D> TfTransformer::lookupTransform(

library/cpp/include/wavemap/core/utils/edit/crop.h

@@ -0,0 +1,125 @@
+#ifndef WAVEMAP_CORE_UTILS_EDIT_CROP_H_
+#define WAVEMAP_CORE_UTILS_EDIT_CROP_H_
+
+#include "wavemap/core/common.h"
+
+namespace wavemap {
+template <typename MapType>
+void cropLeavesBatch(typename MapType::Block::OctreeType::NodeRefType node,
+                     const OctreeIndex& node_index, FloatingPoint& node_value,
+                     const Point3D& t_W_center, FloatingPoint radius,
+                     FloatingPoint min_cell_width) {
+  // Decompress child values
+  using Transform = typename MapType::Block::Transform;
+  auto& node_details = node.data();
+  auto child_values = Transform::backward({node_value, {node_details}});
+
+  // Set all children whose center is outside the cropping sphere to zero
+  for (NdtreeIndexRelativeChild child_idx = 0;
+       child_idx < OctreeIndex::kNumChildren; ++child_idx) {
+    const OctreeIndex child_index = node_index.computeChildIndex(child_idx);
+    const Point3D t_W_child =
+        convert::nodeIndexToCenterPoint(child_index, min_cell_width);
+    const FloatingPoint d_center_child = (t_W_child - t_W_center).norm();
+    if (radius < d_center_child) {
+      child_values[child_idx] = 0;
+      if (0 < child_index.height) {
+        node.eraseChild(child_idx);
+      }
+    }
+  }
+
+  // Compress
+  const auto [new_value, new_details] =
+      HashedChunkedWaveletOctreeBlock::Transform::forward(child_values);
+  node_details = new_details;
+  node_value = new_value;
+}
+
+template <typename MapType>
+void cropNodeRecursive(typename MapType::Block::OctreeType::NodeRefType node,
+                       const OctreeIndex& node_index, FloatingPoint& node_value,
+                       const Point3D& t_W_center, FloatingPoint radius,
+                       FloatingPoint min_cell_width,
+                       IndexElement termination_height) {
+  using NodeRefType = decltype(node);
+
+  // Decompress child values
+  using Transform = typename MapType::Block::Transform;
+  auto& node_details = node.data();
+  auto child_values = Transform::backward({node_value, {node_details}});
+
+  // Handle each child
+  for (NdtreeIndexRelativeChild child_idx = 0;
+       child_idx < OctreeIndex::kNumChildren; ++child_idx) {
+    // If the node is fully inside the cropping sphere, do nothing
+    const auto child_aabb =
+        convert::nodeIndexToAABB(node_index, min_cell_width);
+    if (child_aabb.maxDistanceTo(t_W_center) < radius) {
+      continue;
+    }
+
+    // If the node is fully outside the cropping sphere, set it to zero
+    auto& child_value = child_values[child_idx];
+    if (radius < child_aabb.minDistanceTo(t_W_center)) {
+      child_value = 0;
+      node.eraseChild(child_idx);
+      continue;
+    }
+
+    // Otherwise, continue at a higher resolution
+    NodeRefType child_node = node.getOrAllocateChild(child_idx);
+    const OctreeIndex child_index = node_index.computeChildIndex(child_idx);
+    if (child_index.height <= termination_height + 1) {
+      cropLeavesBatch<MapType>(child_node, child_index, child_value, t_W_center,
+                               radius, min_cell_width);
+    } else {
+      cropNodeRecursive<MapType>(child_node, child_index, child_value,
+                                 t_W_center, radius, min_cell_width,
+                                 termination_height);
+    }
+  }
+
+  // Compress
+  const auto [new_value, new_details] = Transform::forward(child_values);
+  node_details = new_details;
+  node_value = new_value;
+}
+
+template <typename MapType>
+void crop_to_sphere(const Point3D& t_W_center, FloatingPoint radius,
+                    MapType& map, IndexElement termination_height) {
+  const IndexElement tree_height = map.getTreeHeight();
+  const FloatingPoint min_cell_width = map.getMinCellWidth();
+
+  for (auto it = map.getHashMap().begin(); it != map.getHashMap().end();) {
+    // Start by testing at the block level
+    const Index3D& block_index = it->first;
+    const auto block_node_index = OctreeIndex{tree_height, block_index};
+    const auto block_aabb =
+        convert::nodeIndexToAABB(block_node_index, min_cell_width);
+    // If the block is fully inside the cropping sphere, do nothing
+    if (block_aabb.maxDistanceTo(t_W_center) < radius) {
+      ++it;
+      continue;
+    }
+    // If the block is fully outside the cropping sphere, erase it entirely
+    if (radius < block_aabb.minDistanceTo(t_W_center)) {
+      it = map.getHashMap().erase(it);
+      continue;
+    }
+
+    // Since the block overlaps with the sphere's boundary, we need to process
+    // it at a higher resolution by recursing over its cells
+    auto& block = it->second;
+    cropNodeRecursive<MapType>(block.getRootNode(), block_node_index,
+                               block.getRootScale(), t_W_center, radius,
+                               min_cell_width, termination_height);
+    block.setLastUpdatedStamp();
+
+    ++it;
+  }
+}
+}  // namespace wavemap
+
+#endif  // WAVEMAP_CORE_UTILS_EDIT_CROP_H_

library/cpp/src/core/config/value_with_unit.cc

@@ -7,23 +7,33 @@
 namespace wavemap::param {
 // clang-format off
 static const std::map<std::string, std::pair<FloatingPoint, SiUnit::Id>> UnitToSi{
-    {"meters",      {1e0f,        SiUnit::kMeters}},
-    {"m",           {1e0f,        SiUnit::kMeters}},
-    {"decimeters",  {1e-1f,       SiUnit::kMeters}},
-    {"dm",          {1e-1f,       SiUnit::kMeters}},
-    {"centimeters", {1e-2f,       SiUnit::kMeters}},
-    {"cm",          {1e-2f,       SiUnit::kMeters}},
-    {"millimeters", {1e-3f,       SiUnit::kMeters}},
-    {"mm",          {1e-3f,       SiUnit::kMeters}},
-    {"radians",     {1.f,         SiUnit::kRadians}},
-    {"rad",         {1.f,         SiUnit::kRadians}},
-    {"degrees",     {kPi / 180.f, SiUnit::kRadians}},
-    {"deg",         {kPi / 180.f, SiUnit::kRadians}},
-    {"pixels",      {1.f,         SiUnit::kPixels}},
-    {"px",          {1.f,         SiUnit::kPixels}},
-    {"seconds",     {1.f,         SiUnit::kSeconds}},
-    {"sec",         {1.f,         SiUnit::kSeconds}},
-    {"s",           {1.f,         SiUnit::kSeconds}},
+    {"kilometers",   {1e3f,        SiUnit::kMeters}},
+    {"km",           {1e3f,        SiUnit::kMeters}},
+    {"meters",       {1e0f,        SiUnit::kMeters}},
+    {"m",            {1e0f,        SiUnit::kMeters}},
+    {"decimeters",   {1e-1f,       SiUnit::kMeters}},
+    {"dm",           {1e-1f,       SiUnit::kMeters}},
+    {"centimeters",  {1e-2f,       SiUnit::kMeters}},
+    {"cm",           {1e-2f,       SiUnit::kMeters}},
+    {"millimeters",  {1e-3f,       SiUnit::kMeters}},
+    {"mm",           {1e-3f,       SiUnit::kMeters}},
+    {"radians",      {1.f,         SiUnit::kRadians}},
+    {"rad",          {1.f,         SiUnit::kRadians}},
+    {"degrees",      {kPi / 180.f, SiUnit::kRadians}},
+    {"deg",          {kPi / 180.f, SiUnit::kRadians}},
+    {"pixels",       {1.f,         SiUnit::kPixels}},
+    {"px",           {1.f,         SiUnit::kPixels}},
+    {"hours",        {3600.f,      SiUnit::kSeconds}},
+    {"h",            {3600.f,      SiUnit::kSeconds}},
+    {"minutes",      {60.f,        SiUnit::kSeconds}},
+    {"seconds",      {1.f,         SiUnit::kSeconds}},
+    {"sec",          {1.f,         SiUnit::kSeconds}},
+    {"s",            {1.f,         SiUnit::kSeconds}},
+    {"milliseconds", {1e-3f,       SiUnit::kSeconds}},
+    {"ms",           {1e-3f,       SiUnit::kSeconds}},
+    {"microseconds", {1e-6f,       SiUnit::kSeconds}},
+    {"nanoseconds",  {1e-9f,       SiUnit::kSeconds}},
+    {"ns",           {1e-9f,       SiUnit::kSeconds}},
 };
 // clang-format on
 

tooling/schemas/wavemap/map_operations/crop_map_operation.json

@@ -12,14 +12,22 @@
       "$ref": "../value_with_unit/convertible_to_seconds.json"
     },
     "body_frame": {
-      "description": "Name of the TF frame to treat as the center point. Usually the robot's body frame. When the cropper runs, all blocks that are further than remove_blocks_beyond_distance from this point are deleted.",
+      "description": "Name of the TF frame to treat as the center point. Usually the robot's body frame. When the cropper runs, all cells that are further than `radius` from this point are deleted.",
       "type": "string",
       "examples": [
         "body"
       ]
     },
-    "remove_blocks_beyond_distance": {
-      "description": "Distance beyond which blocks are deleted when the cropper is executed.",
+    "tf_time_offset": {
+      "description": "Time offset applied when retrieving the transform from body_frame to world_frame. Set to -1 to use the most recent transform available in ROS TF, ignoring timestamps (default). If set to a non-negative value, the transform lookup uses a timestamp of `ros::Time::now() - tf_time_offset`.",
+      "$ref": "../value_with_unit/convertible_to_seconds.json"
+    },
+    "radius": {
+      "description": "Distance beyond which to remove cells from the map.",
+      "$ref": "../value_with_unit/convertible_to_meters.json"
+    },
+    "max_update_resolution": {
+      "description": "Maximum resolution at which the crop is applied. Set to 0 to match the map's maximum resolution (default). Setting a higher value reduces computation but produces more jagged borders.",
       "$ref": "../value_with_unit/convertible_to_meters.json"
     }
   }

tooling/schemas/wavemap/value_with_unit/convertible_to_meters.json

@@ -5,6 +5,12 @@
   "minProperties": 1,
   "maxProperties": 1,
   "properties": {
+    "kilometers": {
+      "type": "number"
+    },
+    "km": {
+      "type": "number"
+    },
     "meters": {
       "type": "number"
     },