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已经在地图的全局坐标中。

注意：如果只需要浏览边界框内的单元格，请查看方法octree->begin_leafs_bbx()和end_leafs_bbx()来创建迭代器。如果您需要限制叶子的深度，我想您也可以使用这些方法来完成。

我希望这会有所帮助。最好的问候，

Adrián

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

xyx空间中的Octomap（八叉树）坐标

1 个答案: