顾及节点尺寸的octomap叶子节点访问

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/Zed_Of_Zoe/article/details/116888920

目的

octomap以八叉树为基础, 将三维空间划分为一系列立方体, 最大树深度为16.

octomap提供了访问叶子节点的迭代器, 可以通过该迭代器遍历整个空间, 进行相应的计算或处理.

有一些教程提供了示例代码, 如下

for(octomap::OcTree::leaf_iterator it = tree_copy->begin_leafs(tree->getTreeDepth()), end = tree_copy->end_leafs();it != end; ++it)
{
    PointType p;
    p.x = it.getX(), p.y = it.getY(), p.z = it.getZ();
    /*
    自己的计算或处理
    */
}

但octomap会自动地合并节点, 通过迭代器得到的叶子节点的$depth$并不总是16, 导致叶子节点指示的立方体的尺寸并不固定.

在octomap的分辨率为$resolution$时,叶子节点指示的立方体的边长$L$与该叶子节点的深度$depth$的关系为:
$$L=resolution\ast 2^{17-depth}$$
对于$depth$小于16的叶子节点, 如果需要以$resolution$为间隔来访问该节点指示的立方体空间, 就需要将立方体空间平均切分成$8^{16-depth}$个子立方体.

代码

分别在x/y/z方向上, 以$resolution$为间隔来切分叶子节点代表的立方体.

for(octomap::OcTree::leaf_iterator it = tree->begin_leafs(tree->getTreeDepth()), end = tree->end_leafs();it != end; ++it)
{
    PointType p;
    p.x = it.getX(), p.y = it.getY(), p.z = it.getZ();
    float size_half = it.getSize() * 0.5;

    int width = round(size_half / resolution);
    for(int i = -width; i <= width; i++)
    {
        int x = p.x + resolution * i;
        for(int j = -width; j <= width; j++)
        {
            int y = p.y + resolution * j;
            for(int k = -width; k <= width; k++)
            {
                float z = p.z + resolution * k;
                int utility = pow(2,abs(i/width)+abs(j/width)+abs(k/width));
                /*
                自己的计算或处理
                */
            }
        }
    }
}

解释

1. 三层for循环, 将该叶子节点指示的立方体空间以切分成了$8^{16-depth}$个子立方体, 每个子立方体的边长为$resolution\ast 2$
2. 由于for循环取了等于, 因此相邻的叶子节点相交的地方(共边/ 共面/ 共端点)被处理了多次, 用一个$utility$来指示该位置被访问的次数.

int utility = pow(2,abs(i/width)+abs(j/width)+abs(k/width));

图示

图中将访问到的每个子立方体进行了可视化, 所有子立方体的尺寸是一致的, 不同的颜色代表了该子立方体所属的叶子节点的尺寸不同.