版权声明:本文为博主原创文章,遵循 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会自动地合并节点, 通过迭代器得到的叶子节点的 d e p t h depth depth并不总是16, 导致叶子节点指示的立方体的尺寸并不固定.
在octomap的分辨率为
r
e
s
o
l
u
t
i
o
n
resolution
resolution时,叶子节点指示的立方体的边长
L
L
L与该叶子节点的深度
d
e
p
t
h
depth
depth的关系为:
L
=
r
e
s
o
l
u
t
i
o
n
∗
2
17
−
d
e
p
t
h
L= resolution * 2^{17-depth}
L=resolution∗217−depth
对于
d
e
p
t
h
depth
depth小于16的叶子节点, 如果需要以
r
e
s
o
l
u
t
i
o
n
resolution
resolution为间隔来访问该节点指示的立方体空间, 就需要将立方体空间平均切分成
8
16
−
d
e
p
t
h
8^{16-depth}
816−depth个子立方体.
代码
分别在x/y/z方向上, 以 r e s o l u t i o n resolution 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));
/*
自己的计算或处理
*/
}
}
}
}
解释
- 三层for循环, 将该叶子节点指示的立方体空间以切分成了 8 16 − d e p t h 8^{16-depth} 816−depth个子立方体, 每个子立方体的边长为 r e s o l u t i o n ∗ 2 resolution * 2 resolution∗2
- 由于for循环取了等于, 因此相邻的叶子节点相交的地方(共边/ 共面/ 共端点)被处理了多次, 用一个 u t i l i t y utility utility来指示该位置被访问的次数.
int utility = pow(2,abs(i/width)+abs(j/width)+abs(k/width));
图示
图中将访问到的每个子立方体进行了可视化, 所有子立方体的尺寸是一致的, 不同的颜色代表了该子立方体所属的叶子节点的尺寸不同.