deleteAABB: delete an axis-aligned bounding box

Add methods to delete cells either in or out of an axis-aligned
bounding box. This is a common operation for users of the octomap
library, and accelerating it by not visiting nodes to be deleted is
very useful in certain applications, like bounds-limited rolling
costmaps.

Also adds the ability to register a delection callback during node
deletion. This callback is called for every leaf node that is deleted.

Author: C. Andy Martin

octomap/include/octomap/OcTreeBaseImpl.h

@@ -40,6 +40,7 @@
 #include <iterator>
 #include <stack>
 #include <bitset>
+#include <functional>
 
 #include "octomap_types.h"
 #include "OcTreeKey.h"
@@ -223,6 +224,44 @@ namespace octomap {
      */
     bool deleteNode(const OcTreeKey& key, unsigned int depth = 0);
 
+    using DeletionCallback = std::function<void(const OcTreeBaseImpl<NODE, INTERFACE>* tree, const NODE* node, const OcTreeKey& key, unsigned int depth)>;
+
+    /**
+     * Delete all nodes in (or out) of the given AABB.
+     *
+     * Delete all the nodes in the given axis-aligned bounding box (AABB).
+     * This method efficiently descends the tree only visiting the nodes in
+     * (or out) of the box. If invert is set, nodes outside the box are
+     * deleted (exclusive), otherwise nodes inside or on the box are deleted
+     * (inclusive).
+     *
+     * @param min    Bottom-left corner of box (inclusive).
+     * @param max    Top-right corner of box (inclusive).
+     * @param invert If true, delete everything out of the box, otherwise delete in the box.
+     * @param deletion_notifier If set, a callback to call when a leaf node is
+     *               about to be deleted.
+     */
+    void deleteAABB(const point3d& min, const point3d& max, bool invert = false,
+                    DeletionCallback deletion_notifier = DeletionCallback());
+
+    /**
+     * Delete all nodes in (or out) of the given AABB.
+     *
+     * Delete all the nodes in the given axis-aligned bounding box (AABB).
+     * This method efficiently descends the tree only visiting the nodes in
+     * (or out) of the box. If invert is set, nodes outside the box are
+     * deleted (exclusive), otherwise nodes inside or on the box are deleted
+     * (inclusive).
+     *
+     * @param min    Bottom-left corner of box (inclusive).
+     * @param max    Top-right corner of box (inclusive).
+     * @param invert If true, delete everything out of the box, otherwise delete in the box.
+     * @param deletion_notifier If set, a callback to call when a leaf node is
+     *               about to be deleted.
+     */
+    void deleteAABB(const OcTreeKey& min, const OcTreeKey& max, bool invert = false,
+                    DeletionCallback deletion_notifier = DeletionCallback());
+
     /// Deletes the complete tree structure
     void clear();
 
@@ -588,6 +627,18 @@ namespace octomap {
     /// recursive call of deleteNode()
     bool deleteNodeRecurs(NODE* node, unsigned int depth, unsigned int max_depth, const OcTreeKey& key);
 
+    /// Recursively delete a node and all children, calling the deletion_notifier for every leaf node deleted.
+    void deleteNodeRecurs(NODE* node,
+                          const OcTreeKey& key,
+                          unsigned int depth,
+                          const DeletionCallback& deletion_notifier);
+
+    /// recursive call of deleteAABB(). Returns true if node has been deleted.
+    bool deleteAABBRecurs(const OcTreeKey& min, const OcTreeKey& max,
+                          NODE* node, const OcTreeKey& key, unsigned int depth,
+                          unsigned int max_depth, bool invert,
+                          const DeletionCallback& deletion_notifier);
+
     /// recursive call of prune()
     void pruneRecurs(NODE* node, unsigned int depth, unsigned int max_depth, unsigned int& num_pruned);
 
@@ -604,6 +655,10 @@ namespace octomap {
   protected:  
     void allocNodeChildren(NODE* node);
     void deleteNodeChildren(NODE* node);
+    void deleteNodeChildren(NODE* node,
+                            const OcTreeKey& key,
+                            unsigned int depth,
+                            const DeletionCallback& deletion_notifier);
 
     NODE* root; ///< Pointer to the root NODE, NULL for empty tree
 

octomap/include/octomap/OcTreeBaseImpl.hxx

@@ -306,6 +306,23 @@ namespace octomap {
     }
   }
 
+  template <class NODE,class I>
+  void OcTreeBaseImpl<NODE,I>::deleteNodeChildren(NODE* node, const OcTreeKey& key, unsigned int depth,
+                                                  const DeletionCallback& deletion_notifier){
+    if (node->children != NULL) {
+      key_type center_offset_key = tree_max_val >> (depth + 1);
+      for (unsigned int i=0; i<8; i++) {
+        if (node->children[i] != NULL){
+          OcTreeKey child_key;
+          computeChildKey(i, center_offset_key, key, child_key);
+          this->deleteNodeRecurs(static_cast<NODE*>(node->children[i]), child_key, depth + 1, deletion_notifier);
+        }
+      }
+      delete[] node->children;
+      node->children = NULL;
+    }
+  }
+
   template <class NODE,class I>
   inline key_type OcTreeBaseImpl<NODE,I>::coordToKey(double coordinate, unsigned depth) const{
     assert (depth <= tree_depth);
@@ -575,6 +592,24 @@ namespace octomap {
     return deleteNodeRecurs(root, 0, depth, key);
   }
 
+  template <class NODE,class I>
+  void OcTreeBaseImpl<NODE,I>::deleteAABB(const point3d& min, const point3d& max, bool invert,
+                                          DeletionCallback deletion_notifier) {
+    OcTreeKey min_key, max_key;
+    coordToKeyClamped(min, min_key);
+    coordToKeyClamped(max, max_key);
+    deleteAABB(min_key, max_key, invert, deletion_notifier);
+  }
+
+  template <class NODE,class I>
+  void OcTreeBaseImpl<NODE,I>::deleteAABB(const OcTreeKey& min, const OcTreeKey& max, bool invert,
+                                          DeletionCallback deletion_notifier) {
+    octomap::OcTreeKey root_key(tree_max_val, tree_max_val, tree_max_val);
+    if (deleteAABBRecurs(min, max, root, root_key, 0, tree_depth, invert, deletion_notifier)) {
+      root = NULL;
+    }
+  }
+
   template <class NODE,class I>
   void OcTreeBaseImpl<NODE,I>::clear() {
     if (this->root){
@@ -775,6 +810,127 @@ namespace octomap {
     return false;
   }
 
+  template <class NODE,class I>
+  void OcTreeBaseImpl<NODE,I>::deleteNodeRecurs(NODE* node, const OcTreeKey& key, unsigned int depth,
+                                                const DeletionCallback& deletion_notifier){
+    assert(node);
+    assert(deletion_notifier);
+
+    if (!nodeHasChildren(node)) {
+      // Only notify for leaf nodes
+      deletion_notifier(this, node, key, depth);
+    }
+
+    // Always call delete node children in the rare case there is a children
+    // pointer area in an inner node but all children pointers are NULL
+    deleteNodeChildren(node, key, depth, deletion_notifier);
+
+    delete node;
+    tree_size--;
+    size_changed = true;
+  }
+
+  template <class NODE,class I>
+  bool OcTreeBaseImpl<NODE,I>::deleteAABBRecurs(const OcTreeKey& min,
+                                                const OcTreeKey& max,
+                                                NODE* node,
+                                                const OcTreeKey& key,
+                                                unsigned int depth,
+                                                unsigned int max_depth,
+                                                bool invert,
+                                                const DeletionCallback& deletion_notifier) {
+    bool delete_node = false;
+    bool skip_delete_notification = false;
+    if (node != NULL && depth <= tree_depth && depth <= max_depth) {
+#ifndef NDEBUG
+      if (depth < tree_depth)
+      {
+        unsigned int mask = (1 << (tree_depth - depth - 1)) - 1;
+        assert((key[0] & mask) == 0);
+        assert((key[1] & mask) == 0);
+        assert((key[2] & mask) == 0);
+      }
+#endif
+      if (min[0] > max[0] || min[1] > max[1] || min[2] > max[2]) {
+        // The box has no volume.
+        if (invert) {
+          // A non-positive volume box and we are to delete everything outside
+          // of it. Delete the node.
+          delete_node = true;
+        } else {
+          // A non-positive volume box and we are to delete everything inside
+          // of it. Do nothing.
+        }
+      } else {
+        key_type key_size_down = tree_max_val >> depth;
+        key_type key_size_up = (tree_max_val-1) >> depth;
+
+        assert(key[0] - key_size_down <= key[0] + key_size_up);
+        assert(key[1] - key_size_down <= key[1] + key_size_up);
+        assert(key[2] - key_size_down <= key[2] + key_size_up);
+        bool inside = ((min[0] <= (key[0] - key_size_down)) && (max[0] >= (key[0] + key_size_up)) &&
+                       (min[1] <= (key[1] - key_size_down)) && (max[1] >= (key[1] + key_size_up)) &&
+                       (min[2] <= (key[2] - key_size_down)) && (max[2] >= (key[2] + key_size_up)));
+        bool outside = !((min[0] <= (key[0] + key_size_up)) && (max[0] >= (key[0] - key_size_down)) &&
+                         (min[1] <= (key[1] + key_size_up)) && (max[1] >= (key[1] - key_size_down)) &&
+                         (min[2] <= (key[2] + key_size_up)) && (max[2] >= (key[2] - key_size_down)));
+        assert(!(inside && outside));
+        if ((inside && invert) || (outside && !invert)) {
+          // Nothing to do, we are entirely out of the deletion target
+        } else if ((inside && !invert) || (outside && invert)) {
+          // The entire area is inside the deletion area.
+          delete_node = true;
+        } else if (depth < max_depth) {
+          // At this point the current, inner node is both inside and oustide
+          // the bounds (it crosses the boundary).
+          // It is not possible to be at the tree_depth unless somehow at the
+          // tree_depth we were neither inside or outside the bounding keys
+          assert(depth < tree_depth);
+          // Expand if pruned leaf.
+          if (!nodeHasChildren(node)) {
+            expandNode(node);
+          }
+
+          key_type center_offset_key = tree_max_val >> (depth + 1);
+          for (unsigned int i=0; i<8; i++) {
+            if (nodeChildExists(node, i)) {
+              OcTreeKey child_key;
+              computeChildKey(i, center_offset_key, key, child_key);
+
+              if (deleteAABBRecurs(min, max,
+                                   getNodeChild(node, i), child_key, depth + 1,
+                                   max_depth, invert, deletion_notifier)) {
+                setNodeChild(node, i, NULL);
+              }
+            }
+          }
+
+          // If we have no more children left, this inner node can be removed
+          if (!nodeHasChildren(node)) {
+            delete_node = true;
+            // prevent notification, as this is not a leaf
+            skip_delete_notification = true;
+          } else {
+            // It should not be possible to prune this node unless there is a
+            // logic bug above, because we should have deleted at least one of
+            // our children.
+            assert(pruneNode(node) == false);
+            // Update the inner node's expiry to track the min of all children
+            node->updateOccupancyChildren();
+          }
+        }
+      }
+    }
+    if (delete_node) {
+      if (deletion_notifier && !skip_delete_notification) {
+        deleteNodeRecurs(node, key, depth, deletion_notifier);
+      } else {
+        deleteNodeRecurs(node);
+      }
+    }
+		return delete_node;
+  }
+
   template <class NODE,class I>
   void OcTreeBaseImpl<NODE,I>::pruneRecurs(NODE* node, unsigned int depth,
          unsigned int max_depth, unsigned int& num_pruned) {

octomap/src/testing/CMakeLists.txt

@@ -29,6 +29,9 @@ if(BUILD_TESTING)
   ADD_EXECUTABLE(test_bbx test_bbx.cpp)
   TARGET_LINK_LIBRARIES(test_bbx octomap)
 
+  ADD_EXECUTABLE(test_delete_aabb test_delete_aabb.cpp)
+  TARGET_LINK_LIBRARIES(test_delete_aabb octomap octomath)
+
   ADD_EXECUTABLE(test_set_tree_values test_set_tree_values.cpp)
   TARGET_LINK_LIBRARIES(test_set_tree_values octomap octomath)
 
@@ -48,6 +51,7 @@ if(BUILD_TESTING)
   ADD_TEST (NAME test_raycasting    COMMAND test_raycasting)
   ADD_TEST (NAME test_io            COMMAND test_io ${PROJECT_SOURCE_DIR}/share/data/geb079.bt)
   ADD_TEST (NAME test_pruning       COMMAND test_pruning )
+  ADD_TEST (NAME test_delete_aabb   COMMAND test_delete_aabb )
   ADD_TEST (NAME test_set_tree_values COMMAND test_set_tree_values )
   ADD_TEST (NAME test_iterators     COMMAND test_iterators ${PROJECT_SOURCE_DIR}/share/data/geb079.bt)
   ADD_TEST (NAME test_mapcollection COMMAND test_mapcollection ${PROJECT_SOURCE_DIR}/share/data/mapcoll.txt)