From b3ca41bc2eacb19786aa22f030725e5b050f0cec Mon Sep 17 00:00:00 2001 From: Armin Hornung Date: Wed, 3 Apr 2013 15:42:29 +0200 Subject: [PATCH] Fix #16: Nodes are now pruned directly in updateNode. --- octomap/include/octomap/OcTreeDataNode.hxx | 6 +-- octomap/include/octomap/OccupancyOcTreeBase.h | 2 +- .../include/octomap/OccupancyOcTreeBase.hxx | 50 +++++++------------ octomap/include/octomap/octomap_types.h | 1 + octomap/src/OcTreeNode.cpp | 7 ++- octomap/src/graph2tree.cpp | 9 +--- octomap/src/testing/test_pruning.cpp | 4 +- octomap/src/testing/unit_tests.cpp | 13 +++-- 8 files changed, 43 insertions(+), 49 deletions(-) diff --git a/octomap/include/octomap/OcTreeDataNode.hxx b/octomap/include/octomap/OcTreeDataNode.hxx index 18b07a34..267b875b 100644 --- a/octomap/include/octomap/OcTreeDataNode.hxx +++ b/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; diff --git a/octomap/include/octomap/OccupancyOcTreeBase.h b/octomap/include/octomap/OccupancyOcTreeBase.h index 98fcfe38..67fc3376 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.h +++ b/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. diff --git a/octomap/include/octomap/OccupancyOcTreeBase.hxx b/octomap/include/octomap/OccupancyOcTreeBase.hxx index b13eb54d..bdc7cdce 100644 --- a/octomap/include/octomap/OccupancyOcTreeBase.hxx +++ b/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 @@ -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 @@ -290,25 +288,11 @@ namespace octomap { template NODE* OccupancyOcTreeBase::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 @@ -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(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; diff --git a/octomap/include/octomap/octomap_types.h b/octomap/include/octomap/octomap_types.h index f9125c4a..8fb54d34 100644 --- a/octomap/include/octomap/octomap_types.h +++ b/octomap/include/octomap/octomap_types.h @@ -34,6 +34,7 @@ #ifndef OCTOMAP_TYPES_H #define OCTOMAP_TYPES_H +#include #include #include diff --git a/octomap/src/OcTreeNode.cpp b/octomap/src/OcTreeNode.cpp index 32b33385..a6e43193 100644 --- a/octomap/src/OcTreeNode.cpp +++ b/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; diff --git a/octomap/src/graph2tree.cpp b/octomap/src/graph2tree.cpp index 2fd3e929..2f837574 100644 --- a/octomap/src/graph2tree.cpp +++ b/octomap/src/graph2tree.cpp @@ -57,7 +57,6 @@ void printUsage(char* self){ " -m (optional) \n" " -n (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 (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); diff --git a/octomap/src/testing/test_pruning.cpp b/octomap/src/testing/test_pruning.cpp index 6c8ebc24..4f9b231d 100644 --- a/octomap/src/testing/test_pruning.cpp +++ b/octomap/src/testing/test_pruning.cpp @@ -86,7 +86,8 @@ 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()); @@ -94,6 +95,7 @@ int main(int argc, char** argv) { 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){ diff --git a/octomap/src/testing/unit_tests.cpp b/octomap/src/testing/unit_tests.cpp index 4a0620d8..08198517 100644 --- a/octomap/src/testing/unit_tests.cpp +++ b/octomap/src/testing/unit_tests.cpp @@ -279,6 +279,7 @@ 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 " <getTimestamp() << std::endl; EXPECT_TRUE (tree_time > 0); #ifdef _WIN32 Sleep(1000); @@ -286,12 +287,18 @@ int main(int argc, char** argv) { sleep(1); #endif stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update + std::cout << "After 2nd update (single miss): Tree time " <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);