UpdateList renamed to KeySet, all updateNode functions public

include/octomap/OccupancyOcTreeBase.h

@@ -78,7 +78,7 @@ namespace octomap {
      * @note you need to use boost::unordered_set instead if your compiler does not
      * yet support tr1
      */
-    typedef std::tr1::unordered_set<OcTreeKey, OcTreeKey::KeyHash> UpdateList;
+    typedef std::tr1::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
 
     OccupancyOcTreeBase(double _resolution);
     virtual ~OccupancyOcTreeBase();
@@ -132,6 +132,7 @@ namespace octomap {
 
     /**
      * Integrate occupancy measurement.
+     * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
      *
      * @param value 3d coordinate of the NODE that is to be updated
      * @param occupied true if the node was measured occupied, else false
@@ -142,7 +143,8 @@ namespace octomap {
     virtual NODE* updateNode(const point3d& value, bool occupied, bool dirty = false);
 
     /**
-     * Manipulate log_odds value of voxel directly
+     * Manipulate log_odds value of voxel directly.
+     * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
      *
      * @param value 3d coordinate of the NODE that is to be updated
      * @param log_odds_update value to be added (+) to log_odds value of node
@@ -152,6 +154,27 @@ namespace octomap {
      */
     virtual NODE* updateNode(const point3d& value, float log_odds_update, bool dirty = false);
 
+    /**
+     * Integrate occupancy measurement.
+     *
+     * @param OcTreeKey of the NODE that is to be updated
+     * @param occupied true if the node was measured occupied, else false
+     * @param dirty whether the tree is left 'dirty' after the update (default: false).
+     *   This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.
+     * @return pointer to the updated NODE
+     */
+    virtual NODE* updateNode(const OcTreeKey& key, bool occupied, bool dirty = false);
+
+    /**
+     * Manipulate log_odds value of voxel directly
+     *
+     * @param OcTreeKey of the NODE that is to be updated
+     * @param log_odds_update value to be added (+) to log_odds value of node
+     * @param dirty whether the tree is left 'dirty' after the update (default: false).
+     *   This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.
+     * @return pointer to the updated NODE
+     */
+    virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool dirty = false);
 
     /// Creates the maximum likelihood map by calling toMaxLikelihood on all
     /// tree nodes, setting their occupancy to the corresponding occupancy thresholds.
@@ -269,12 +292,21 @@ namespace octomap {
     /// sets the maximum threshold for occupancy clamping (sensor model)
     void setClampingThresMax(double thresProb){clampingThresMax = logodds(thresProb); }
 
-    /// Helper for insertScanUniform (internal use). Computes all free and occupied nodes
-    /// required for the update at once. Here, occupied nodes have a preference over free
-    /// ones.
+
+    /**
+     * Helper for insertScan. Computes all octree nodes affected by the point cloud
+     * integration at once. Here, occupied nodes have a preference over free
+     * ones.
+     *
+     * @param scan point cloud measurement to be integrated
+     * @param origin origin of the sensor for ray casting
+     * @param free_cells keys of nodes to be cleared
+     * @param occupied_cells keys of nodes to be marked occupied
+     * @param maxrange maximum range for raycasting (-1: unlimited)
+     */
     void computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
-                        UpdateList& free_cells,
-                        UpdateList& occupied_cells,
+                        KeySet& free_cells,
+                        KeySet& occupied_cells,
                        double maxrange);
 
     // -- I/O  -----------------------------------------
@@ -370,28 +402,6 @@ namespace octomap {
 
   protected:
 
-    /**
-     * Integrate occupancy measurement.
-     *
-     * @param OcTreeKey of the NODE that is to be updated
-     * @param occupied true if the node was measured occupied, else false
-     * @param dirty whether the tree is left 'dirty' after the update (default: false).
-     *   This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.
-     * @return pointer to the updated NODE
-     */
-    NODE* updateNode(const OcTreeKey& key, bool occupied, bool dirty = false);
-
-    /**
-     * Manipulate log_odds value of voxel directly
-     *
-     * @param OcTreeKey of the NODE that is to be updated
-     * @param log_odds_update value to be added (+) to log_odds value of node
-     * @param dirty whether the tree is left 'dirty' after the update (default: false).
-     *   This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.
-     * @return pointer to the updated NODE
-     */
-    NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool dirty = false);
-
     /**
      * Traces a ray from origin to end and updates all voxels on the
      *  way as free.  The volume containing "end" is not updated.

include/octomap/OccupancyOcTreeBase.hxx

@@ -88,14 +88,14 @@ namespace octomap {
   void OccupancyOcTreeBase<NODE>::insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin, 
                                              double maxrange, bool pruning, bool dirty) {
 
-    UpdateList free_cells, occupied_cells;
+    KeySet free_cells, occupied_cells;
     computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);    
 
     // insert data into tree  -----------------------
-    for (UpdateList::iterator it = free_cells.begin(); it != free_cells.end(); it++) {
+    for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); it++) {
       updateNode(*it, false, dirty);
     }
-    for (UpdateList::iterator it = occupied_cells.begin(); it != occupied_cells.end(); it++) {
+    for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); it++) {
       updateNode(*it, true, dirty);
     }
 
@@ -134,8 +134,8 @@ namespace octomap {
 
   template <class NODE>
   void OccupancyOcTreeBase<NODE>::computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
-                                                UpdateList& free_cells,
-                                                UpdateList& occupied_cells,
+                                                KeySet& free_cells,
+                                                KeySet& occupied_cells,
                                                 double maxrange) {
 
     for (Pointcloud::const_iterator point_it = scan.begin(); point_it != scan.end(); point_it++) {
@@ -192,7 +192,8 @@ namespace octomap {
 
     } // end for all points
 
-    for(UpdateList::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ){
+    // prefer occupied cells over free ones (and make sets disjunct)
+    for(KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ){
       if (occupied_cells.find(*it) != occupied_cells.end()){
         it = free_cells.erase(it);
       } else{
@@ -281,6 +282,7 @@ namespace octomap {
     else {
       if (occupied) integrateHit(node);
       else          integrateMiss(node);
+
       return node;
     }
   }