Generalize map crop method to other shapes

examples/cpp/edit/sum_map.cc

@@ -4,8 +4,8 @@
 #include <wavemap/core/utils/edit/crop.h>
 #include <wavemap/core/utils/edit/sum.h>
 #include <wavemap/core/utils/edit/transform.h>
-#include <wavemap/core/utils/geometry/aabb.h>
-#include <wavemap/core/utils/geometry/sphere.h>
+#include <wavemap/core/utils/shape/aabb.h>
+#include <wavemap/core/utils/shape/sphere.h>
 #include <wavemap/io/file_conversions.h>
 
 using namespace wavemap;  // NOLINT

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

@@ -83,17 +83,18 @@ void CropMapOperation::run(bool force_run) {
 
   // Crop the map
   timer_.start();
+  const Sphere cropping_sphere{T_W_B->getPosition(), config_.radius};
   if (auto* hashed_wavelet_octree =
           dynamic_cast<HashedWaveletOctree*>(occupancy_map_.get());
       hashed_wavelet_octree) {
-    edit::crop_to_sphere(*hashed_wavelet_octree, T_W_B->getPosition(),
-                         config_.radius, termination_height_, thread_pool_);
+    edit::crop(*hashed_wavelet_octree, cropping_sphere, termination_height_,
+               thread_pool_);
   } else if (auto* hashed_chunked_wavelet_octree =
                  dynamic_cast<HashedChunkedWaveletOctree*>(
                      occupancy_map_.get());
              hashed_chunked_wavelet_octree) {
-    edit::crop_to_sphere(*hashed_chunked_wavelet_octree, T_W_B->getPosition(),
-                         config_.radius, termination_height_, thread_pool_);
+    edit::crop(*hashed_chunked_wavelet_octree, cropping_sphere,
+               termination_height_, thread_pool_);
   } else {
     ROS_WARN(
         "Map cropping is only supported for hash-based map data structures.");

library/cpp/include/wavemap/core/indexing/index_conversions.h

@@ -9,8 +9,8 @@
 #include "wavemap/core/indexing/ndtree_index.h"
 #include "wavemap/core/utils/bits/morton_encoding.h"
 #include "wavemap/core/utils/data/eigen_checks.h"
-#include "wavemap/core/utils/geometry/aabb.h"
 #include "wavemap/core/utils/math/int_math.h"
+#include "wavemap/core/utils/shape/aabb.h"
 
 namespace wavemap::convert {
 template <int dim>

library/cpp/include/wavemap/core/integrator/projection_model/projector_base.h

@@ -7,7 +7,7 @@
 #include "wavemap/core/common.h"
 #include "wavemap/core/config/type_selector.h"
 #include "wavemap/core/config/value_with_unit.h"
-#include "wavemap/core/utils/geometry/aabb.h"
+#include "wavemap/core/utils/shape/aabb.h"
 
 namespace wavemap {
 struct ProjectorType : TypeSelector<ProjectorType> {

library/cpp/include/wavemap/core/integrator/projective/coarse_to_fine/range_image_intersector.h

@@ -11,7 +11,7 @@
 #include "wavemap/core/integrator/projection_model/projector_base.h"
 #include "wavemap/core/integrator/projective/coarse_to_fine/hierarchical_range_bounds.h"
 #include "wavemap/core/integrator/projective/update_type.h"
-#include "wavemap/core/utils/geometry/aabb.h"
+#include "wavemap/core/utils/shape/aabb.h"
 
 namespace wavemap {
 class RangeImageIntersector {

library/cpp/include/wavemap/core/integrator/projective/fixed_resolution/fixed_resolution_integrator.h

@@ -6,7 +6,7 @@
 
 #include "wavemap/core/data_structure/image.h"
 #include "wavemap/core/integrator/projective/projective_integrator.h"
-#include "wavemap/core/utils/geometry/aabb.h"
+#include "wavemap/core/utils/shape/aabb.h"
 
 namespace wavemap {
 class FixedResolutionIntegrator : public ProjectiveIntegrator {

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

@@ -9,24 +9,21 @@
 
 namespace wavemap::edit {
 namespace detail {
-template <typename MapT>
+template <typename MapT, typename ShapeT>
 void cropLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node,
                      const OctreeIndex& node_index, FloatingPoint& node_value,
-                     const Point3D& t_W_center, FloatingPoint radius,
-                     FloatingPoint min_cell_width);
+                     ShapeT&& shape, FloatingPoint min_cell_width);
 
-template <typename MapT>
+template <typename MapT, typename ShapeT>
 void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node,
                        const OctreeIndex& node_index, FloatingPoint& node_value,
-                       const Point3D& t_W_center, FloatingPoint radius,
-                       FloatingPoint min_cell_width,
+                       ShapeT&& shape, FloatingPoint min_cell_width,
                        IndexElement termination_height = 0);
 }  // namespace detail
 
-template <typename MapT>
-void crop_to_sphere(MapT& map, const Point3D& t_W_center, FloatingPoint radius,
-                    IndexElement termination_height = 0,
-                    const std::shared_ptr<ThreadPool>& thread_pool = nullptr);
+template <typename MapT, typename ShapeT>
+void crop(MapT& map, ShapeT shape, IndexElement termination_height = 0,
+          const std::shared_ptr<ThreadPool>& thread_pool = nullptr);
 }  // namespace wavemap::edit
 
 #include "wavemap/core/utils/edit/impl/crop_inl.h"

library/cpp/include/wavemap/core/utils/edit/impl/crop_inl.h

@@ -4,27 +4,26 @@
 #include <memory>
 
 #include "wavemap/core/indexing/index_conversions.h"
+#include "wavemap/core/utils/shape/intersection_tests.h"
 
 namespace wavemap::edit {
 namespace detail {
-template <typename MapT>
+template <typename MapT, typename ShapeT>
 void cropLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node,
                      const OctreeIndex& node_index, FloatingPoint& node_value,
-                     const Point3D& t_W_center, FloatingPoint radius,
-                     FloatingPoint min_cell_width) {
+                     ShapeT&& shape, FloatingPoint min_cell_width) {
   // Decompress child values
   using Transform = typename MapT::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
+  // Set all children whose center is outside the cropping shape 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) {
+    if (!shape::is_inside(t_W_child, shape)) {
       child_values[child_idx] = 0;
       if (0 < child_index.height) {
         node.eraseChild(child_idx);
@@ -39,11 +38,10 @@ void cropLeavesBatch(typename MapT::Block::OctreeType::NodeRefType node,
   node_value = new_value;
 }
 
-template <typename MapT>
+template <typename MapT, typename ShapeT>
 void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node,
                        const OctreeIndex& node_index, FloatingPoint& node_value,
-                       const Point3D& t_W_center, FloatingPoint radius,
-                       FloatingPoint min_cell_width,
+                       ShapeT&& shape, FloatingPoint min_cell_width,
                        IndexElement termination_height) {
   using NodeRefType = decltype(node);
 
@@ -55,16 +53,16 @@ void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node,
   // 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
+    // If the node is fully inside the cropping shape, do nothing
     const auto child_aabb =
         convert::nodeIndexToAABB(node_index, min_cell_width);
-    if (child_aabb.maxDistanceTo(t_W_center) < radius) {
+    if (shape::is_inside(child_aabb, shape)) {
       continue;
     }
 
-    // If the node is fully outside the cropping sphere, set it to zero
+    // If the node is fully outside the cropping shape, set it to zero
     auto& child_value = child_values[child_idx];
-    if (radius < child_aabb.minDistanceTo(t_W_center)) {
+    if (!shape::overlaps(child_aabb, shape)) {
       child_value = 0;
       node.eraseChild(child_idx);
       continue;
@@ -74,11 +72,11 @@ void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node,
     NodeRefType child_node = node.getOrAllocateChild(child_idx);
     const OctreeIndex child_index = node_index.computeChildIndex(child_idx);
     if (child_index.height <= termination_height + 1) {
-      cropLeavesBatch<MapT>(child_node, child_index, child_value, t_W_center,
-                            radius, min_cell_width);
+      cropLeavesBatch<MapT>(child_node, child_index, child_value, shape,
+                            min_cell_width);
     } else {
-      cropNodeRecursive<MapT>(child_node, child_index, child_value, t_W_center,
-                              radius, min_cell_width, termination_height);
+      cropNodeRecursive<MapT>(child_node, child_index, child_value, shape,
+                              min_cell_width, termination_height);
     }
   }
 
@@ -89,10 +87,9 @@ void cropNodeRecursive(typename MapT::Block::OctreeType::NodeRefType node,
 }
 }  // namespace detail
 
-template <typename MapT>
-void crop_to_sphere(MapT& map, const Point3D& t_W_center, FloatingPoint radius,
-                    IndexElement termination_height,
-                    const std::shared_ptr<ThreadPool>& thread_pool) {
+template <typename MapT, typename ShapeT>
+void crop(MapT& map, ShapeT shape, IndexElement termination_height,
+          const std::shared_ptr<ThreadPool>& thread_pool) {
   using NodePtrType = typename MapT::Block::OctreeType::NodePtrType;
   const IndexElement tree_height = map.getTreeHeight();
   const FloatingPoint min_cell_width = map.getMinCellWidth();
@@ -104,18 +101,18 @@ void crop_to_sphere(MapT& map, const Point3D& t_W_center, FloatingPoint radius,
     const OctreeIndex block_node_index{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) {
+    // If the block is fully inside the cropping shape, do nothing
+    if (shape::is_inside(block_aabb, shape)) {
       ++it;
       continue;
     }
-    // If the block is fully outside the cropping sphere, erase it entirely
-    if (radius < block_aabb.minDistanceTo(t_W_center)) {
+    // If the block is fully outside the cropping shape, erase it entirely
+    if (!shape::overlaps(block_aabb, shape)) {
       it = map.getHashMap().erase(it);
       continue;
     }
 
-    // Since the block overlaps with the sphere's boundary, we need to process
+    // Since the block overlaps with the shape's boundary, we need to process
     // it at a higher resolution by recursing over its cells
     auto& block = it->second;
     // Indicate that the block has changed
@@ -126,17 +123,17 @@ void crop_to_sphere(MapT& map, const Point3D& t_W_center, FloatingPoint radius,
     NodePtrType root_node_ptr = &block.getRootNode();
     // Recursively crop all nodes
     if (thread_pool) {
-      thread_pool->add_task([root_node_ptr, block_node_index, root_value_ptr,
-                             t_W_center, radius, min_cell_width,
+      thread_pool->add_task([&shape, root_node_ptr, block_node_index,
+                             root_value_ptr, min_cell_width,
                              termination_height]() {
         detail::cropNodeRecursive<MapT>(*root_node_ptr, block_node_index,
-                                        *root_value_ptr, t_W_center, radius,
-                                        min_cell_width, termination_height);
+                                        *root_value_ptr, shape, min_cell_width,
+                                        termination_height);
       });
     } else {
       detail::cropNodeRecursive<MapT>(*root_node_ptr, block_node_index,
-                                      *root_value_ptr, t_W_center, radius,
-                                      min_cell_width, termination_height);
+                                      *root_value_ptr, shape, min_cell_width,
+                                      termination_height);
     }
     // Advance to the next block
     ++it;

library/cpp/include/wavemap/core/utils/edit/impl/sum_inl.h

@@ -6,8 +6,8 @@
 #include <utility>
 
 #include "wavemap/core/indexing/index_conversions.h"
-#include "wavemap/core/utils/geometry/aabb.h"
 #include "wavemap/core/utils/iterate/grid_iterator.h"
+#include "wavemap/core/utils/shape/aabb.h"
 
 namespace wavemap::edit {
 namespace detail {
@@ -188,8 +188,8 @@ void sum(MapT& map_A, const MapT& map_B,
   }
 }
 
-template <typename MapT, typename IndicatorFn>
-void sum(MapT& map, IndicatorFn shape, FloatingPoint update,
+template <typename MapT, typename ShapeT>
+void sum(MapT& map, ShapeT shape, FloatingPoint update,
          const std::shared_ptr<ThreadPool>& thread_pool) {
   // Find the blocks that overlap with the shape
   const FloatingPoint block_width =

library/cpp/include/wavemap/core/utils/edit/impl/transform_inl.h

@@ -9,9 +9,9 @@
 #include "wavemap/core/indexing/index_conversions.h"
 #include "wavemap/core/indexing/index_hashes.h"
 #include "wavemap/core/utils/edit/sum.h"
-#include "wavemap/core/utils/geometry/aabb.h"
 #include "wavemap/core/utils/iterate/grid_iterator.h"
 #include "wavemap/core/utils/query/map_interpolator.h"
+#include "wavemap/core/utils/shape/aabb.h"
 
 namespace wavemap::edit {
 template <typename MapT>

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

@@ -40,8 +40,8 @@ template <typename MapT>
 void sum(MapT& map_A, const MapT& map_B,
          const std::shared_ptr<ThreadPool>& thread_pool = nullptr);
 
-template <typename MapT, typename IndicatorFn>
-void sum(MapT& map, IndicatorFn shape, FloatingPoint update,
+template <typename MapT, typename ShapeT>
+void sum(MapT& map, ShapeT shape, FloatingPoint update,
          const std::shared_ptr<ThreadPool>& thread_pool = nullptr);
 }  // namespace wavemap::edit
 

library/cpp/include/wavemap/core/utils/query/point_sampler.h

@@ -5,10 +5,10 @@
 #include <utility>
 
 #include "wavemap/core/common.h"
-#include "wavemap/core/utils/geometry/aabb.h"
 #include "wavemap/core/utils/query/classified_map.h"
 #include "wavemap/core/utils/query/occupancy.h"
 #include "wavemap/core/utils/random_number_generator.h"
+#include "wavemap/core/utils/shape/aabb.h"
 
 namespace wavemap {
 class PointSampler {

library/cpp/include/wavemap/core/utils/geometry/aabb.h → library/cpp/include/wavemap/core/utils/shape/aabb.h

@@ -1,5 +1,5 @@
-#ifndef WAVEMAP_CORE_UTILS_GEOMETRY_AABB_H_
-#define WAVEMAP_CORE_UTILS_GEOMETRY_AABB_H_
+#ifndef WAVEMAP_CORE_UTILS_SHAPE_AABB_H_
+#define WAVEMAP_CORE_UTILS_SHAPE_AABB_H_
 
 #include <algorithm>
 #include <limits>
@@ -129,4 +129,4 @@ struct AABB {
 };
 }  // namespace wavemap
 
-#endif  // WAVEMAP_CORE_UTILS_GEOMETRY_AABB_H_
+#endif  // WAVEMAP_CORE_UTILS_SHAPE_AABB_H_

library/cpp/include/wavemap/core/utils/shape/intersection_tests.h

@@ -0,0 +1,29 @@
+#ifndef WAVEMAP_CORE_UTILS_SHAPE_INTERSECTION_TESTS_H_
+#define WAVEMAP_CORE_UTILS_SHAPE_INTERSECTION_TESTS_H_
+
+#include "wavemap/core/utils/shape/aabb.h"
+#include "wavemap/core/utils/shape/sphere.h"
+
+namespace wavemap::shape {
+template <typename ShapeT, int dim>
+bool is_inside(const Point<dim>& point, const ShapeT& shape) {
+  return shape.contains(point);
+}
+
+template <typename PointT>
+bool is_inside(const AABB<PointT>& aabb, const Sphere<PointT>& sphere) {
+  return sphere.contains(aabb.min) && sphere.contains(aabb.max);
+}
+
+template <typename PointT>
+bool overlaps(const AABB<PointT>& aabb, const Sphere<PointT>& sphere) {
+  const PointT closest_point_in_aabb = aabb.closestPointTo(sphere.center);
+  return sphere.contains(closest_point_in_aabb);
+}
+template <typename PointT>
+bool overlaps(const Sphere<PointT>& sphere, const AABB<PointT>& aabb) {
+  return overlaps(aabb, sphere);
+}
+}  // namespace wavemap::shape
+
+#endif  // WAVEMAP_CORE_UTILS_SHAPE_INTERSECTION_TESTS_H_

library/cpp/include/wavemap/core/utils/geometry/sphere.h → library/cpp/include/wavemap/core/utils/shape/sphere.h

@@ -1,5 +1,5 @@
-#ifndef WAVEMAP_CORE_UTILS_GEOMETRY_SPHERE_H_
-#define WAVEMAP_CORE_UTILS_GEOMETRY_SPHERE_H_
+#ifndef WAVEMAP_CORE_UTILS_SHAPE_SPHERE_H_
+#define WAVEMAP_CORE_UTILS_SHAPE_SPHERE_H_
 
 #include <algorithm>
 #include <string>
@@ -45,4 +45,4 @@ struct Sphere {
 };
 }  // namespace wavemap
 
-#endif  // WAVEMAP_CORE_UTILS_GEOMETRY_SPHERE_H_
+#endif  // WAVEMAP_CORE_UTILS_SHAPE_SPHERE_H_

library/cpp/test/src/core/data_structure/test_aabb.cc

@@ -5,8 +5,8 @@
 #include <gtest/gtest.h>
 
 #include "wavemap/core/common.h"
-#include "wavemap/core/utils/geometry/aabb.h"
 #include "wavemap/core/utils/print/eigen.h"
+#include "wavemap/core/utils/shape/aabb.h"
 #include "wavemap/test/fixture_base.h"
 #include "wavemap/test/geometry_generator.h"
 

library/python/src/edit.cc

@@ -9,8 +9,8 @@
 #include <wavemap/core/utils/edit/crop.h>
 #include <wavemap/core/utils/edit/multiply.h>
 #include <wavemap/core/utils/edit/transform.h>
-#include <wavemap/core/utils/geometry/aabb.h>
-#include <wavemap/core/utils/geometry/sphere.h>
+#include <wavemap/core/utils/shape/aabb.h>
+#include <wavemap/core/utils/shape/sphere.h>
 
 using namespace nb::literals;  // NOLINT
 

library/python/src/geometry.cc

@@ -2,8 +2,8 @@
 
 #include <nanobind/eigen/dense.h>
 #include <wavemap/core/common.h>
-#include <wavemap/core/utils/geometry/aabb.h>
-#include <wavemap/core/utils/geometry/sphere.h>
+#include <wavemap/core/utils/shape/aabb.h>
+#include <wavemap/core/utils/shape/sphere.h>
 
 using namespace nb::literals;  // NOLINT