How to iterate over all leaf nodes of a specific parent node?

161 views

Skip to first unread message

Sebastian Eck

unread,

Jan 21, 2019, 4:10:52 PM1/21/19

to OctoMap developers and users discussion

Hello everyone,

I would like to multithread my octomap processing by running one thread per depth 2 node (assuming root node is depth 1 and its eight children are depth 2), so each thread could handle one eighth of the map. I know I could simply traverse the tree with nodeHasChildren and getNodeChild, but by design a node does not store any information about it's position. I need this information because I have to address adjacent nodes, thus I need some sort of iterator with an OcTreeKey to navigate to adjacent nodes. However I could not find a trivial way to achieve this. Does anybody know how to properly iterate over all descendants of a specific node so that I have an OcTreeKey for each traversed node? For clarification please see my code below:

octomap::OcTree* mapPtr;

void travTreeRec(octomap::OcTreeNode* nodePtr) {
    
    if (mapPtr->nodeHasChildren(nodePtr)) {
    
    for (int i = 0; i < 8; ++i) {
        
        if (mapPtr->nodeChildExists(nodePtr, i))    travTreeRec(mapPtr->getNodeChild(nodePtr, i));
        
    }
    
    } else if (mapPtr->isNodeOccupied(nodePtr)) {
    
    // node is an occupied leaf node, now find adjacent nodes
    
    }
    
}

void* travTree(void* voidArgsPtr) {
    
    travTreeRec((octomap::OcTreeNode*) voidArgsPtr);
    
}

int main(int argc, char **argv) {
    
    mapPtr = new octomap::OcTree("/home/fapsros/Downloads/fr_campus.bt");
    octomap::OcTreeNode* rootNodePtr = mapPtr->getRoot();
    
    pthread_t threads[8];
    
    for (int i = 0; i < 8; ++i) {
        if (mapPtr->nodeChildExists(rootNodePtr, i)) {
        octomap::OcTreeNode* childNodePtr = mapPtr->getNodeChild(rootNodePtr, i);
        pthread_create(&threads[i], nullptr, travTree, (void*) childNodePtr);
        pthread_join(threads[i], nullptr);
        }
    }
    
    return 0;
    

}