问xyx空间中的八叉图(八叉树)坐标
EN

Stack Overflow用户

提问于 2018-06-25 18:08:25

回答 1查看 1.4K关注 0票数 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;
}

不出所料，当我传入一个从空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值并不像预期的那样对应于单个小球体。这是什么转换？

ros

octree

moveit

octomap

c++

回答 1

Stack Overflow用户

发布于 2018-07-03 14:33:45

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

深度从树根开始计数，因此您显示为输出的单元格是高级单元格，由于其大小，通常不应用于冲突检查目的(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;
    }
}