From d0f2966ffc5106936ed3ac71ec003bd12276d426 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Fran=C3=A7ois-Michel=20De=20Rainville?= Date: Tue, 4 Nov 2014 09:02:24 -0500 Subject: [PATCH 1/3] Added a mark free tool --- octomap/src/mark_free.cpp | 160 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 160 insertions(+) create mode 100644 octomap/src/mark_free.cpp diff --git a/octomap/src/mark_free.cpp b/octomap/src/mark_free.cpp new file mode 100644 index 00000000..ea326e77 --- /dev/null +++ b/octomap/src/mark_free.cpp @@ -0,0 +1,160 @@ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace octomap; + +class USizeOcTree : public OcTree { +public: + USizeOcTree(double size) : OcTree(size){} + + void updateSize(){ + this->tree_size = count_nodes(this->getRoot()); + } + +protected: + size_t count_nodes(OcTreeNode* node){ + size_t count = 1; + + for(unsigned int i = 0; i < 8; ++i){ + if(node->childExists(i)){ + count += count_nodes(node->getChild(i)); + } + } + + return count; + } + +}; + +unsigned int mark_free(OcTreeNode* node, unsigned int depth, unsigned int max_depth, float clamping_thres_min, float occ_prob_thres_log){ + if(node->getLogOdds() <= occ_prob_thres_log) + node->setLogOdds(clamping_thres_min); + + if(depth == max_depth) + return 1; + + unsigned int count = 0; + bool any_child_exist = false; + + for(unsigned int i = 0; i < 8; ++i){ + if(node->childExists(i)){ + any_child_exist = true; + break; + } + } + + if(any_child_exist){ + for(unsigned int i = 0; i < 8; ++i){ + if(!node->childExists(i)){ + node->createChild(i); + } + + count += mark_free(node->getChild(i), depth + 1, max_depth, clamping_thres_min, occ_prob_thres_log); + } + } + + // prevent the tree from hugging all the memory + if(node->collapsible()) + node->pruneNode(); + + return count; +} + +unsigned int mark_free(USizeOcTree* tree) { + float clamping_thres_min = tree->getClampingThresMinLog(); + float occ_prob_thres_log = tree->getOccupancyThresLog(); + + // prevent float equality ambiguity + // a single occupied hit sets the logodds way above that number + // for most configuration (default is 0.85) + occ_prob_thres_log += 1e-6; + unsigned int max_depth = tree->getTreeDepth(); + + unsigned int count = 0; + OcTreeNode* root = tree->getRoot(); + + for(unsigned int i = 0; i < 8; ++i){ + if(!root->childExists(i)){ + root->createChild(i); + } + + count += mark_free(root->getChild(i), 2, max_depth, clamping_thres_min, occ_prob_thres_log); + } + + tree->prune(); + + // We added a few nodes + tree->updateSize(); + + return count; +} + +int main(int argc, char** argv) { + bool show_help = false; + string outputFilename(""); + string inputFilename(""); + + if(argc == 1) show_help = true; + for(int i = 1; i < argc && !show_help; i++) { + if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 || + strcmp(argv[i], "--usage") == 0 || strcmp(argv[i], "-usage") == 0 || + strcmp(argv[i], "-h") == 0 + ) + show_help = true; + } + + if(show_help) { + cout << "Usage: "<" << endl; + cout << "\t -o Output filename (default: first input filename + .bt)" << endl; + exit(0); + } + + for(int i = 1; i < argc; i++) { + // Parse command line arguments + if(strcmp(argv[i], "-o") == 0 && i < argc - 1) { + i++; + outputFilename = argv[i]; + continue; + } else if (i == argc-1){ + inputFilename = string(argv[i]); + } + } + + if (outputFilename == ""){ + size_t lastdot = inputFilename.find_last_of("."); + if (lastdot == std::string::npos) + outputFilename = inputFilename + ".free.bt"; + else + outputFilename = inputFilename.substr(0, lastdot) + ".free.bt"; + } + + // An udatable size octree + USizeOcTree* tree = new USizeOcTree(0.1); + if (!tree->readBinary(inputFilename)){ + OCTOMAP_ERROR("Could not open file, exiting.\n"); + exit(1); + } + + cout << "Creating all necessary nodes and marking them free" << endl; + unsigned int count = mark_free(tree); + cout << "Marked " << count << " leaves as free" << endl; + + // Must convert the Updatable Size OcTree to an OcTree + // for writing + OcTree* otree = tree; + cout << "Writing octree to " << outputFilename << endl; + if (!otree->writeBinary(outputFilename)){ + OCTOMAP_ERROR("Error writing tree to %s\n", outputFilename.c_str()); + exit(1); + } + + cout << "done" << endl; + delete tree; + return 0; +} \ No newline at end of file From 8ff3d9acf2f71664fa8ac97949a4c066b8781ae2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Fran=C3=A7ois-Michel=20De=20Rainville?= Date: Tue, 4 Nov 2014 09:02:43 -0500 Subject: [PATCH 2/3] Added the marke free tool to build list --- octomap/src/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/octomap/src/CMakeLists.txt b/octomap/src/CMakeLists.txt index aa24a188..46cb4047 100644 --- a/octomap/src/CMakeLists.txt +++ b/octomap/src/CMakeLists.txt @@ -39,6 +39,9 @@ TARGET_LINK_LIBRARIES(bt2vrml octomap) ADD_EXECUTABLE(edit_octree edit_octree.cpp) TARGET_LINK_LIBRARIES(edit_octree octomap) +ADD_EXECUTABLE(mark_free mark_free.cpp) +TARGET_LINK_LIBRARIES(mark_free octomap) + ADD_EXECUTABLE(convert_octree convert_octree.cpp) TARGET_LINK_LIBRARIES(convert_octree octomap) @@ -71,6 +74,7 @@ install(TARGETS binvox2bt bt2vrml edit_octree + mark_free convert_octree eval_octree_accuracy compare_octrees From e141d208d0874381b5b06c93f3266118e5e4f300 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Fran=C3=A7ois-Michel=20De=20Rainville?= Date: Tue, 4 Nov 2014 12:19:08 -0500 Subject: [PATCH 3/3] Correct the depth of the root node. --- octomap/src/mark_free.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/octomap/src/mark_free.cpp b/octomap/src/mark_free.cpp index ea326e77..4b478050 100644 --- a/octomap/src/mark_free.cpp +++ b/octomap/src/mark_free.cpp @@ -84,7 +84,7 @@ unsigned int mark_free(USizeOcTree* tree) { root->createChild(i); } - count += mark_free(root->getChild(i), 2, max_depth, clamping_thres_min, occ_prob_thres_log); + count += mark_free(root->getChild(i), 1, max_depth, clamping_thres_min, occ_prob_thres_log); } tree->prune();