c++ - xyx空间中的Octomap(八叉树)坐标

Question

给定一个 octomap::OcTree , 如何获取被占用单元格的笛卡尔坐标？

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;
}

当我传入从空 PlanningScene 生成的 octree 时正如预期的那样，我得到 0 个占用的单元格。当我使用的场景已知在 xyz 坐标(0.1、0.8、0.1)处有一个半径为 0.05 米的球体时，根据场景的引用系(也是米)，我得到以下输出:

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

肯定需要进行一些转换，因为这些 octomap xyz 值并不像预期的那样对应于单个小球体。这是什么转化？

Answer 1

我发现问题出在您使用迭代器的方式上。八叉树具有树的结构，您使用的迭代器类型在树中导航而不考虑单元的深度。

深度从树根开始计数，因此您显示为输出的单元格是高级单元格，通常不应将其用于碰撞检查目的，因为它们的大小(depth=1 是树根树，它包含 4 个深度为 2 的单元...并且递归地进行直到 max_depth，通常为 16)。

我知道您想知道哪些叶单元(较小的单元)被占用，并且您有一个迭代器可以帮助您这样做。这是我的做法:

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;
    }
}

不需要转换，xyz已经在 map 的全局坐标中。

注意:如果您只需要在边界框内的单元格中导航，请查看方法 octree->begin_leafs_bbx() 和 end_leafs_bbx()创建你的迭代器。如果您需要限制叶子的深度，我认为您也可以使用这些方法来做到这一点。

希望对您有所帮助。最好的问候，

阿德里安

编辑:由于 begin_leafs() 的返回类型错误，更改了答案中的代码。另外，请注意 begin_leafs() 和 end_leafs() 与 begin() 和 end() 具有相同的行为> 根据 Octomap API。