Octomap (octree) coordinates in xyx space

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?

Adrián González · Answer 1 · 2018-07-11T11:28:01.487

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.

This attempt, although with `OcTree::leaf_iterator`, unfortunately gives me the same strange coordinates. When I use `OcTree::leaf_bbx_iterator` as you suggest, I get a compile error: `error: conversion from ‘octomap::OcTreeBaseImpl::leaf_iterator’ to non-scalar type ‘octomap::OcTreeBaseImpl::leaf_bbx_iterator’ requested` — BoltzmannBrain, Jul 04 '18 at 00:25
Sorry, that was a mistake in the code I posted. As you said, `begin_leafs()` returns an OcTree::leaf_iterator. Also, taking a deeper look at the API, it turns out that `begin()` and `end()` have the same behavior than `begin_leafs()` and `end_leafs()`. My bad. — Adrián González, Jul 11 '18 at 10:35
(continues) What is clear is that there is no need of converting the coordinates to meters. But maybe you can try to export your .ot file and inspect it with octovis to see that the information contained in it makes sense... because the rest of the code looks good to me. — Adrián González, Jul 11 '18 at 11:26
With a leaf iterator I still get depths in the range [1, 6], and the tree's max depth is 16. Yes I want the deep leaf coordinates only, as these will give me the precise collision-checking info. This iterator approach doesn't appear to work though, or am I misunderstanding something? — BoltzmannBrain, Jul 20 '18 at 17:28

Octomap (octree) coordinates in xyx space

1 Answers1