Fix #16: Nodes are now pruned directly in updateNode.

octomap/include/octomap/OcTreeDataNode.hxx

@@ -148,10 +148,10 @@ namespace octomap {
     if (!childExists(0) || getChild(0)->hasChildren())
       return false;
 
-    T childValue = getChild(0)->getValue();
-
     for (unsigned int i = 1; i<8; i++) {
-      if (!childExists(i) || getChild(i)->hasChildren() || !(getChild(i)->getValue() == childValue))
+      // comparison via getChild so that casts of derived classes ensure
+      // that the right == operator gets called
+      if (!childExists(i) || getChild(i)->hasChildren() || !(*(getChild(i)) == *(getChild(0))))
         return false;
     }
     return true;

octomap/include/octomap/OccupancyOcTreeBase.h

@@ -122,7 +122,7 @@ namespace octomap {
     virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false);
 
     /// @note Deprecated, use insertScanRays instead.
-    OCTOMAP_DEPRECATED( virtual void insertScanNaive(const Pointcloud& svan, const point3d& sensor_origin, double maxrange, bool pruning = true, bool lazy_eval = false));
+    OCTOMAP_DEPRECATED( virtual void insertScanNaive(const Pointcloud& scan, const point3d& sensor_origin, double maxrange, bool pruning = true, bool lazy_eval = false));
 
     /**
     * Integrate a Pointcloud (in global reference frame), parallelized with OpenMP.

octomap/include/octomap/OccupancyOcTreeBase.hxx

@@ -93,11 +93,7 @@ namespace octomap {
     for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
       updateNode(*it, true, lazy_eval);
     }
-
-    // TODO: does pruning make sense if we used "lazy_eval"?
-    if (pruning)
-      this->prune();
-  } 
+  }
 
   // performs transformation to data and sensor origin first
   template <class NODE>
@@ -262,12 +258,14 @@ namespace octomap {
       return leaf;
     }
 
+    bool createdRoot = false;
     if (this->root == NULL){
       this->root = new NODE();
       this->tree_size++;
+      createdRoot = true;
     }
 
-    return updateNodeRecurs(this->root, false, key, 0, log_odds_update, lazy_eval);
+    return updateNodeRecurs(this->root, createdRoot, key, 0, log_odds_update, lazy_eval);
   }
 
   template <class NODE>
@@ -290,25 +288,11 @@ namespace octomap {
 
   template <class NODE>
   NODE* OccupancyOcTreeBase<NODE>::updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval) {
-    // early abort (no change will happen).
-    // may cause an overhead in some configuration, but more often helps
-    NODE* leaf = this->search(key);
-    if (leaf
-        && ((occupied && leaf->getLogOdds() >= this->clamping_thres_max)
-        || ( !occupied && leaf->getLogOdds() <= this->clamping_thres_min)))
-    {
-      return leaf;
-    }
-
-    if (this->root == NULL){
-      this->root = new NODE();
-      this->tree_size++;
-    }
-
+    float logOdds = this->prob_miss_log;
     if (occupied)
-      return updateNodeRecurs(this->root, false, key, 0, this->prob_hit_log,  lazy_eval);
-    else
-      return updateNodeRecurs(this->root, false, key, 0, this->prob_miss_log, lazy_eval);
+      logOdds = this->prob_hit_log;
+
+    return updateNode(key, logOdds, lazy_eval);
   }
 
   template <class NODE>
@@ -339,9 +323,8 @@ namespace octomap {
     if (depth < this->tree_depth) {
       if (!node->childExists(pos)) {
         // child does not exist, but maybe it's a pruned node?
-        if ((!node->hasChildren()) && !node_just_created && (node != this->root)) {
+        if ((!node->hasChildren()) && !node_just_created ) {
           // current node does not have children AND it is not a new node 
-          // AND its not the root node // TODO: last check could be eliminated?
           // -> expand pruned node
           node->expandNode();
           this->tree_size+=8;
@@ -360,10 +343,13 @@ namespace octomap {
         return updateNodeRecurs(node->getChild(pos), created_node, key, depth+1, log_odds_update, lazy_eval);
       else {
         NODE* retval = updateNodeRecurs(node->getChild(pos), created_node, key, depth+1, log_odds_update, lazy_eval);
-        // set own probability according to prob of children
-        //if (node->pruneNode())
-        // TODO: pruning here?
-        node->updateOccupancyChildren();
+        // prune node if possible, otherwise set own probability
+        // note: combining both did not lead to a speedup!
+        if (node->pruneNode())
+          this->tree_size -= 8;
+        else
+          node->updateOccupancyChildren();
+
         return retval;
       }
     }
@@ -373,6 +359,7 @@ namespace octomap {
       if (use_change_detection) {
         bool occBefore = this->isNodeOccupied(node);
         updateNodeLogOdds(node, log_odds_update); 
+
         if (node_just_created){  // new node
           changed_keys.insert(std::pair<OcTreeKey,bool>(key, true));
         } else if (occBefore != this->isNodeOccupied(node)) {  // occupancy changed, track it
@@ -382,8 +369,7 @@ namespace octomap {
           else if (it->second == false)
             changed_keys.erase(it);
         }
-      }
-      else {
+      } else {
         updateNodeLogOdds(node, log_odds_update); 
       }
       return node;

octomap/include/octomap/octomap_types.h

@@ -34,6 +34,7 @@
 #ifndef OCTOMAP_TYPES_H
 #define OCTOMAP_TYPES_H
 
+#include <stdio.h>
 #include <vector>
 #include <list>
 

octomap/src/OcTreeNode.cpp

@@ -73,7 +73,9 @@ namespace octomap {
         c++;
       }
     }
-    if (c) mean /= (double) c;
+    if (c)
+      mean /= (double) c;
+
     return log(mean/(1-mean));
   }
 
@@ -82,7 +84,8 @@ namespace octomap {
     for (unsigned int i=0; i<8; i++) {
       if (childExists(i)) {
         float l = getChild(i)->getLogOdds();
-        if (l > max) max = l;
+        if (l > max)
+          max = l;
       }
     }
     return max;

octomap/src/graph2tree.cpp

@@ -57,7 +57,6 @@ void printUsage(char* self){
             "  -m <maxrange> (optional) \n"
             "  -n <max scan no.> (optional) \n"
             "  -log (enable a detailed log file with statistics) \n"
-            "  -compress (enable lossless compression after every scan)\n"
             "  -compressML (enable maximum-likelihood compression (lossy) after every scan)\n"
             "  -simple (simple scan insertion ray by ray instead of optimized) \n"
             "  -clamping <p_min> <p_max> (override default sensor model clamping probabilities between 0..1)\n"
@@ -111,7 +110,7 @@ int main(int argc, char** argv) {
   int max_scan_no = -1;
   bool detailedLog = false;
   bool simpleUpdate = false;
-  unsigned char compression = 0;
+  unsigned char compression = 1;
 
   // get default sensor model values:
   OcTree emptyTree(0.1);
@@ -139,7 +138,7 @@ int main(int argc, char** argv) {
     else if (! strcmp(argv[arg], "-simple"))
       simpleUpdate = true;
     else if (! strcmp(argv[arg], "-compress"))
-      compression = 1;
+      OCTOMAP_WARNING("Argument -compress no longer has an effect, incremental pruning is done during each insertion.\n");
     else if (! strcmp(argv[arg], "-compressML"))
       compression = 2;
     else if (! strcmp(argv[arg], "-m"))
@@ -274,11 +273,7 @@ int main(int argc, char** argv) {
   cout << "time to insert 100.000 points took: " << time_to_insert/ ((double) num_points_in_graph / 100000) << " sec (avg)" << endl << endl;
 
 
-  std::cout << "Full tree\n" << "===========================\n";
-  outputStatistics(tree);
-
   std::cout << "Pruned tree (lossless compression)\n" << "===========================\n";
-  tree->prune();
   outputStatistics(tree);
 
   tree->write(treeFilenameOT);

octomap/src/testing/test_pruning.cpp

@@ -86,14 +86,16 @@ int main(int argc, char** argv) {
       }
     }
     EXPECT_EQ(tree.calcNumNodes(), tree.size());
-    EXPECT_EQ(37477, tree.size());
+    EXPECT_EQ(29, tree.size());
+    // TODO: replace with test for lazy eval?
     tree.prune();
     EXPECT_EQ(tree.calcNumNodes(), tree.size());
     EXPECT_EQ(29, tree.size());
     tree.expand();
     EXPECT_EQ(tree.calcNumNodes(), tree.size());
     EXPECT_EQ(37477, tree.size());
     tree.prune();
+    EXPECT_EQ(29, tree.size());
     // test expansion:
     for (float x=0.005; x <= 0.32; x+=res){
       for (float y=0.005; y <= 0.32; y+=res){

octomap/src/testing/unit_tests.cpp

@@ -279,19 +279,26 @@ int main(int argc, char** argv) {
     EXPECT_TRUE (result);
     unsigned int tree_time = stamped_tree.getLastUpdateTime();
     unsigned int node_time = result->getTimestamp();
+    std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
     EXPECT_TRUE (tree_time > 0);
     #ifdef _WIN32
       Sleep(1000);
     #else
       sleep(1);
     #endif
     stamped_tree.integrateMissNoTime(result);  // reduce occupancy, no time update
+    std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
     EXPECT_EQ  (node_time, result->getTimestamp()); // node time updated?
-    query = point3d  (0.1f, 0.1f, 0.3f);
-    stamped_tree.updateNode(query, true); // integrate 'occupied' measurement 
-    OcTreeNodeStamped* result2 = stamped_tree.search (query);
+    point3d query2 = point3d  (0.1f, 0.1f, 0.3f);
+    stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement
+    OcTreeNodeStamped* result2 = stamped_tree.search (query2);
     EXPECT_TRUE (result2);
+    result = stamped_tree.search (query);
+    EXPECT_TRUE (result);
+    std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp()
+        << "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
     EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated
+    EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
   // ------------------------------------------------------------
   } else if (test_name == "OcTreeKey") {
     OcTree tree (0.05);