OcTree bounding box pruning

perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

@@ -105,7 +105,7 @@ bool DepthImageOctomapUpdater::setParams(XmlRpc::XmlRpcValue &params)
 bool DepthImageOctomapUpdater::initialize()
 {
   tf_ = monitor_->getTFClient();
-  free_space_updater_.reset(new LazyFreeSpaceUpdater(tree_));
+  free_space_updater_.reset(new LazyFreeSpaceUpdater(tree_, monitor_));
 
   // create our mesh filter
   mesh_filter_.reset(new mesh_filter::MeshFilter<mesh_filter::StereoCameraModel>(mesh_filter::MeshFilterBase::TransformCallback(),
@@ -422,10 +422,11 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP
   }
 
   // figure out occupied cells and model cells
-  tree_->lockRead();
+
 
   try
   {
+    ReadLock lock = monitor_->readingMap();
     const int h_bound = h - skip_vertical_pixels_;
     const int w_bound = w - skip_horizontal_pixels_;
 
@@ -491,19 +492,17 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP
   }
   catch (...)
   {
-    tree_->unlockRead();
     return;
   }
-  tree_->unlockRead();
 
   /* cells that overlap with the model are not occupied */
   for (octomap::KeySet::iterator it = model_cells.begin(), end = model_cells.end(); it != end; ++it)
     occupied_cells.erase(*it);
 
   // mark occupied cells
-  tree_->lockWrite();
   try
   {
+    WriteLock lock = monitor_->writingMap();
     /* now mark all occupied cells */
     for (octomap::KeySet::iterator it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
       tree_->updateNode(*it, true);
@@ -512,8 +511,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::ImageConstP
   {
     ROS_ERROR("Internal error while updating octree");
   }
-  tree_->unlockWrite();
-  tree_->triggerUpdateCallback();
+  monitor_->triggerUpdateCallback();
 
   // at this point we still have not freed the space
   free_space_updater_->pushLazyUpdate(occupied_cells_ptr, model_cells_ptr, sensor_origin);

perception/lazy_free_space_updater/include/moveit/lazy_free_space_updater/lazy_free_space_updater.h

@@ -38,6 +38,7 @@
 #define MOVEIT_OCCUPANCY_MAP_MONITOR_LAZY_FREE_SPACE_UPDATER_
 
 #include <moveit/occupancy_map_monitor/occupancy_map.h>
+#include <moveit/occupancy_map_monitor/occupancy_map_monitor.h>
 #include <boost/thread.hpp>
 #include <deque>
 
@@ -48,7 +49,7 @@ class LazyFreeSpaceUpdater
 {
 public:
 
-  LazyFreeSpaceUpdater(const OccMapTreePtr &tree, unsigned int max_batch_size = 10);
+  LazyFreeSpaceUpdater(const OccMapTreePtr &tree, OccupancyMapMonitor *monitor, unsigned int max_batch_size = 10);
   ~LazyFreeSpaceUpdater();
 
   void pushLazyUpdate(octomap::KeySet *occupied_cells, octomap::KeySet *model_cells, const octomap::point3d &sensor_origin);
@@ -67,6 +68,7 @@ class LazyFreeSpaceUpdater
   void processThread();
 
   OccMapTreePtr tree_;
+  OccupancyMapMonitor *monitor_;
   bool running_;
   std::size_t max_batch_size_;
   double max_sensor_delta_;

perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp

@@ -40,11 +40,12 @@
 namespace occupancy_map_monitor
 {
 
-LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr &tree, unsigned int max_batch_size) :
+LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const OccMapTreePtr &tree, OccupancyMapMonitor *monitor, unsigned int max_batch_size) :
   tree_(tree),
   running_(true),
   max_batch_size_(max_batch_size),
   max_sensor_delta_(1e-3), // 1mm
+  monitor_(monitor),
   process_occupied_cells_set_(NULL),
   process_model_cells_set_(NULL),
   update_thread_(boost::bind(&LazyFreeSpaceUpdater::lazyUpdateThread, this)),
@@ -121,32 +122,32 @@ void LazyFreeSpaceUpdater::processThread()
     ROS_DEBUG("Begin processing batched update: marking free cells due to %lu occupied cells and %lu model cells", (long unsigned int)process_occupied_cells_set_->size(), (long unsigned int)process_model_cells_set_->size());
 
     ros::WallTime start = ros::WallTime::now();
-    tree_->lockRead();
+    {
+      ReadLock lock = monitor_->readingMap();
 
 #pragma omp sections
-        {
+      {
 
 #pragma omp section
-      {
-        /* compute the free cells along each ray that ends at an occupied cell */
-        for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
-          if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it->first), key_ray1))
-            for (octomap::KeyRay::iterator jt = key_ray1.begin(), end = key_ray1.end() ; jt != end ; ++jt)
-              free_cells1[*jt] += it->second;
-      }
+        {
+          /* compute the free cells along each ray that ends at an occupied cell */
+          for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
+            if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(it->first), key_ray1))
+              for (octomap::KeyRay::iterator jt = key_ray1.begin(), end = key_ray1.end() ; jt != end ; ++jt)
+                free_cells1[*jt] += it->second;
+        }
 
 #pragma omp section
-      {
-        /* compute the free cells along each ray that ends at a model cell */
-        for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
-          if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(*it), key_ray2))
-            for (octomap::KeyRay::iterator jt = key_ray2.begin(), end = key_ray2.end() ; jt != end ; ++jt)
-              free_cells2[*jt]++;
+        {
+          /* compute the free cells along each ray that ends at a model cell */
+          for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
+            if (tree_->computeRayKeys(process_sensor_origin_, tree_->keyToCoord(*it), key_ray2))
+              for (octomap::KeyRay::iterator jt = key_ray2.begin(), end = key_ray2.end() ; jt != end ; ++jt)
+                free_cells2[*jt]++;
+        }
       }
     }
 
-    tree_->unlockRead();
-
     for (OcTreeKeyCountMap::iterator it = process_occupied_cells_set_->begin(), end = process_occupied_cells_set_->end(); it != end; ++it)
     {
       free_cells1.erase(it->first);
@@ -160,10 +161,9 @@ void LazyFreeSpaceUpdater::processThread()
     }
     ROS_DEBUG("Marking %lu cells as free...", (long unsigned int)(free_cells1.size() + free_cells2.size()));
 
-    tree_->lockWrite();
-
     try
     {
+      WriteLock lock = monitor_->writingMap();
       // set the logodds to the minimum for the cells that are part of the model
       for (octomap::KeySet::iterator it = process_model_cells_set_->begin(), end = process_model_cells_set_->end(); it != end; ++it)
         tree_->updateNode(*it, lg_0);
@@ -178,8 +178,7 @@ void LazyFreeSpaceUpdater::processThread()
     {
       ROS_ERROR("Internal error while updating octree");
     }
-    tree_->unlockWrite();
-    tree_->triggerUpdateCallback();
+    monitor_->triggerUpdateCallback();
 
     ROS_DEBUG("Marked free cells in %lf ms", (ros::WallTime::now() - start).toSec() * 1000.0);
 

perception/occupancy_map_monitor/CMakeLists.txt

@@ -1,9 +1,10 @@
 set(MOVEIT_LIB_NAME moveit_occupancy_map_monitor)
 
 add_library(${MOVEIT_LIB_NAME}
+  src/occupancy_map.cpp
   src/occupancy_map_monitor.cpp
   src/occupancy_map_updater.cpp
-  )
+)
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib)

perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map.h

@@ -32,15 +32,13 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Ioan Sucan, Jon Binney */
+/* Author: Ioan Sucan, Jon Binney, Connor Brew */
 
 #ifndef MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_
 #define MOVEIT_OCCUPANCY_MAP_MONITOR_OCCUPANCY_MAP_
 
 #include <octomap/octomap.h>
 #include <boost/shared_ptr.hpp>
-#include <boost/thread/shared_mutex.hpp>
-#include <boost/function.hpp>
 
 namespace occupancy_map_monitor
 {
@@ -50,69 +48,57 @@ typedef octomap::OcTreeNode OccMapNode;
 class OccMapTree : public octomap::OcTree
 {
 public:
+  OccMapTree(double resolution) : octomap::OcTree(resolution), bbx_size(0.0), bbx_height(0.0) {}
+  OccMapTree(double resolution, double size) : octomap::OcTree(resolution), bbx_size(size), bbx_height(size) {}
+  OccMapTree(double resolution, double size, double height) : octomap::OcTree(resolution), bbx_size(size), bbx_height(height) {}
+  OccMapTree(const std::string &filename) : octomap::OcTree(filename), bbx_size(0.0), bbx_height(0.0) {}
+  OccMapTree(const OccMapTree& rhs) : octomap::OcTree(rhs), bbx_size(rhs.bbx_size), bbx_height(rhs.bbx_height) {}
 
-  OccMapTree(double resolution) : octomap::OcTree(resolution)
+  void pruneBBX()
   {
+    if (!root || bbx_size <= 0.0)
+      return;
+    octomap::OcTreeKey root_key;
+    root_key[0] = root_key[1] = root_key[2] = tree_max_val;
+    if (pruneBBXRecurs(root, root_key, 0))
+      tree_size = this->calcNumNodes();
   }
 
-  OccMapTree(const std::string &filename) : octomap::OcTree(filename)
+  void setBBXCenter(double x, double y, double z)
   {
+    octomap::point3d min(x-bbx_size, y-bbx_size, z-bbx_height);
+    octomap::point3d max(x+bbx_size, y+bbx_size, z+bbx_height);
+    setBBXMin(min);
+    setBBXMax(max);
   }
 
-  /** @brief lock the underlying octree. it will not be read or written by the
-   *  monitor until unlockTree() is called */
-  void lockRead()
+  void setBBXSize(double size)
   {
-    tree_mutex_.lock_shared();
+    bbx_size = size;
   }
 
-  /** @brief unlock the underlying octree. */
-  void unlockRead()
+  double getBBXSize()
   {
-    tree_mutex_.unlock_shared();
+    return bbx_size;
   }
 
-  /** @brief lock the underlying octree. it will not be read or written by the
-   *  monitor until unlockTree() is called */
-  void lockWrite()
+  void setBBXHeight(double height)
   {
-    tree_mutex_.lock();
+    bbx_height = height;
   }
 
-  /** @brief unlock the underlying octree. */
-  void unlockWrite()
+  double getBBXHeight()
   {
-    tree_mutex_.unlock();
+    return bbx_height;
   }
 
-  typedef boost::shared_lock<boost::shared_mutex> ReadLock;
-  typedef boost::unique_lock<boost::shared_mutex> WriteLock;
+protected:
+  bool inBBX(const octomap::OcTreeKey& key, unsigned int depth) const;
+  bool inBBXStrict(const octomap::OcTreeKey& key, unsigned int depth) const;
+  bool pruneBBXRecurs(octomap::OcTreeNode* node, const octomap::OcTreeKey& parent_key, unsigned int depth);
 
-  ReadLock reading()
-  {
-    return ReadLock(tree_mutex_);
-  }
-
-  WriteLock writing()
-  {
-    return WriteLock(tree_mutex_);
-  }
-
-  void triggerUpdateCallback(void)
-  {
-    if (update_callback_)
-      update_callback_();
-  }
-
-  /** @brief Set the callback to trigger when updates are received */
-  void setUpdateCallback(const boost::function<void()> &update_callback)
-  {
-    update_callback_ = update_callback;
-  }
-
-private:
-  boost::shared_mutex tree_mutex_;
-  boost::function<void()> update_callback_;
+  double bbx_size;
+  double bbx_height;
 };
 
 typedef boost::shared_ptr<OccMapTree> OccMapTreePtr;

perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h

@@ -49,10 +49,15 @@
 #include <moveit/occupancy_map_monitor/occupancy_map_updater.h>
 
 #include <boost/thread/mutex.hpp>
+#include <boost/thread/shared_mutex.hpp>
+#include <boost/function.hpp>
 
 namespace occupancy_map_monitor
 {
 
+typedef boost::shared_lock<boost::shared_mutex> ReadLock;
+typedef boost::unique_lock<boost::shared_mutex> WriteLock;
+
 class OccupancyMapMonitor
 {
 public:
@@ -107,10 +112,17 @@ class OccupancyMapMonitor
   /** \brief Forget about this shape handle and the shapes it corresponds to */
   void forgetShape(ShapeHandle handle);
 
+  /** @brief Signal that the occupancy map has been updated */
+  void triggerUpdateCallback()
+  {
+    if (update_callback_)
+      update_callback_();
+  }
+
   /** @brief Set the callback to trigger when updates to the maintained octomap are received */
   void setUpdateCallback(const boost::function<void()> &update_callback)
   {
-    tree_->setUpdateCallback(update_callback);
+    update_callback_ = update_callback;
   }
 
   void setTransformCacheCallback(const TransformCacheProvider &transform_cache_callback);
@@ -122,6 +134,42 @@ class OccupancyMapMonitor
     return active_;
   }
 
+  /** @brief lock the underlying octree. it will not be read or written by the
+   *  monitor until unlockTree() is called */
+  void lockMapRead()
+  {
+    map_mutex_.lock_shared();
+  }
+
+  /** @brief unlock the underlying octree. */
+  void unlockMapRead()
+  {
+    map_mutex_.unlock_shared();
+  }
+
+  /** @brief lock the underlying octree. it will not be read or written by the
+   *  monitor until unlockTree() is called */
+  void lockMapWrite()
+  {
+    map_mutex_.lock();
+  }
+
+  /** @brief unlock the underlying octree. */
+  void unlockMapWrite()
+  {
+    map_mutex_.unlock();
+  }
+
+  ReadLock readingMap()
+  {
+    return ReadLock(map_mutex_);
+  }
+
+  WriteLock writingMap()
+  {
+    return WriteLock(map_mutex_);
+  }
+
 private:
 
   void initialize();
@@ -138,6 +186,7 @@ class OccupancyMapMonitor
   std::string map_frame_;
   double map_resolution_;
   boost::mutex parameters_lock_;
+  boost::shared_mutex map_mutex_;
 
   OccMapTreePtr tree_;
   OccMapTreeConstPtr tree_const_;
@@ -146,6 +195,7 @@ class OccupancyMapMonitor
   std::vector<OccupancyMapUpdaterPtr> map_updaters_;
   std::vector<std::map<ShapeHandle, ShapeHandle> > mesh_handles_;
   TransformCacheProvider transform_cache_callback_;
+  boost::function<void()> update_callback_;
   bool debug_info_;
 
   std::size_t mesh_handle_count_;