Augmenting Octomap nodes with additional info

I'm interested in doing the following things with Octomap. I'm not sure whether they can be accomplished by using the templated classes, or by rewriting the Octomap source. What is the best solution, and which parts of the code do I need to look into?

1. Adding additional information to the nodes in the Octomap tree - for example, color.

2. Being able to query the value at different resolution levels. So I'd need a function which returns which returns the mean (or mode, or min/max etc) of the value from all the children nodes.

3. Visualizing the voxels based on their value - preferably in rviz

Thanks for the help!

edit retag close delete

Sort by » oldest newest most voted

1. Because both the octrees and nodes are templated, that should be really easy to do as dornhege wrote. You would need to derive your own OcTreeNode (for data) and OcTree (for implementation). In the last versions, we moved more and more of the actual occupancy implementation into OccupancyOcTreeBase, so you can directly derive from that. For examples, have a look at the OcTreeNodeStamped and OcTreeStamped.

2. See the answer by dornhege. For that to work with the max depth, you would need to keep the inner tree values consistent with the max or mean of their children, as it's done in OcTreeNode after each update.

3. Check octomap_server in octomap_mapping, it sends out a visualization as MarkerArray for RViz that you can easily adapt to your needs. It's not the most efficient way to display all the voxels in RViz (and they're not shaded properly), so for much data you might need to extend our own viewer octovis to your needs.

more

Note: I didn't look in the current version's source. For the 0.8 I am working with:

1. You probably need to write your own code (given you want occupany mapping) as the default OcTreeNode is already a OcTreeDataNote<float>. It is quite simple though, as you can just derive the classes and make sure the new node class is instantiated in the tree.

2. That's definitely possible as you pass a max_depth to the querying function and then the inner node's value will be returned.

more