1

给定一个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;
}

因为当我传入一个octree由空生成的PlanningScene单元格时，我得到了 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 值并不像预期的那样对应于单个小球体。这个转换是什么？

4

1 回答 1

1

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

深度从树根开始计算，因此您显示为输出的单元格是高级单元格，通常不应将其用于碰撞检查，因为它们的大小（深度 = 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 已经在地图的全局坐标中。

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

我希望这有帮助。此致，

阿德里安

编辑：由于返回类型的错误，更改了答案中的代码begin_leafs()。另外，请注意begin_leafs()并且具有与Octomap APIend_leafs()相同的行为。begin()end()

于 2018-07-03T14:33:45.490 回答