Octomap (octree) coordinates in xyx space - c++

Question

Given an octomap::OcTree, how can I get the cartesian coordinates of the occupied cells?
double printOccupied(boost::shared_ptr<octomap::OcTree> octree) {
// Get some octomap config data
auto res = octree->getResolution();
unsigned int max_depth = octree->getTreeDepth();
// Iterate over nodes
int count = 0;
std::cout << "printOccupied: octree res = " << res << std::endl;
std::cout << "printOccupied: octree max depth = " << max_depth << std::endl;
std::cout << "printOccupied: iterating over nodes..." << std::endl;
for (octomap::OcTree::iterator it = octree->begin(); it != octree->end(); ++it) {
if (octree->isNodeOccupied(*it) && it.getDepth() < max_depth) {
count++;
// Fetching the coordinates in octomap-space
std::cout << " x = " << it.getX() << std::endl;
std::cout << " y = " << it.getY() << std::endl;
std::cout << " z = " << it.getZ() << std::endl;
std::cout << " size = " << it.getSize() << std::endl;
std::cout << " depth = " << it.getDepth() << std::endl;
// Then convert to meters???
auto cell = std::make_tuple(it.getX() * res,
it.getY() * res,
it.getZ() * res);
}
}
std::cout << "printOccupied: number of occupied cells = " << count << std::endl;
}
For when I pass in an octree that is generated from an empty PlanningScene I get 0 occupied cells, as expected. When I use a scene that is known to have a single sphere of radius 0.05 meters at xyz coordinates (0.1, 0.8, 0.1), according to the scene's reference frame (also meters), I get the following output:
printOccupied: octree res = 0.02
printOccupied: octree max depth = 16
printOccupied: iterating over nodes...
x = -327.68
y = -327.68
z = -327.68
size = 655.36
depth = 1
x = 327.68
y = -327.68
z = -327.68
size = 655.36
depth = 1
x = -491.52
y = 491.52
z = -491.52
size = 327.68
depth = 2
x = 327.68
y = 327.68
z = -327.68
size = 655.36
depth = 1
x = -92.16
y = 624.64
z = 51.2
size = 20.48
depth = 6
x = -81.92
y = 409.6
z = 245.76
size = 163.84
depth = 3
x = -419.84
y = 624.64
z = 378.88
size = 20.48
depth = 6
x = -409.6
y = 409.6
z = 573.44
size = 163.84
depth = 3
x = 327.68
y = 327.68
z = 327.68
size = 655.36
depth = 1
printOccupied: number of occupied cells = 9
Surely there must be some conversion needed, as these octomap xyz values do not correspond to a single small sphere as expected. What is this conversion?

Answer 1

I see that the problem is the way in which you are using iterators. An octree has the structure of a tree, and the kind of iterator you are using navigates through the tree without taking into account the depth of the cells.
The depth counts from the tree root, so the cells that you show as output are high-level cells which should not be used, in general, for collision check purposes because of their size (depth=1 is the root of the tree, which contains 4 cells of depth 2... and that goes on recursively until max_depth, which is normally 16).
I understand that you want to know which leaf cells (the smaller ones) are occupied, and you have one iterator which might help you to do so. Here is how I do it:
for(OcTree::leaf_iterator it = octree->begin_leafs(), end = octree->end_leafs(); it != end; ++it){
// Fetching the coordinates in octomap-space
std::cout << " x = " << it.getX() << std::endl;
std::cout << " y = " << it.getY() << std::endl;
std::cout << " z = " << it.getZ() << std::endl;
std::cout << " size = " << it.getSize() << std::endl;
std::cout << " depth = " << it.getDepth() << std::endl;
}
}
No conversion is needed, xyz are already in global coordinates of the map.
Note: If you need to navigate only through cells which are inside a bounding box, take a look to the methods octree->begin_leafs_bbx() and end_leafs_bbx() to create your iterator. If you need to limit the depth of the leafs I think you can do that with those methods too.
I hope this helps. Best regards,
Adrián
EDIT: changed the code in the answer because of an error in the return type of begin_leafs(). Also, noted that begin_leafs() and end_leafs() have the same behavior than begin() and end() according to the Octomap API.