八叉树与体素图
八叉树地图
八叉树地图是可变分辨率的三维栅格地图，可以自由调整分辨率，如下所示：
根据点云的数量或密度决定每个叶子方块是否被占据
体素图
体素就是固定分辨率的三维栅格地图，如下所示：
根据点云的数量或密度决定每个体素是否被占据
八叉树地图与体素图的区别
最大的区别是八叉树地图是可变分辨率，体素图是固定分辨率，也就是说，在一定大小的三维空间内，体素图是按照最小的分割体积，将空间完整分割表达，数据结构简单，不过存储量更多一些，但是建图速度更快。
八叉树地图使用八叉树的数据结构来存储，对于一些没有点云或点云数量极少的空区域，可以不进行展开分割，只存储这一大块体积，而对点云有效，密集的区域进行进一步的分割存储，也就是所说的可变分辨率，这样做减少存储量，但数据结构复杂，通常情况下建图速度没有体素图快
八叉树地图适合做大地图的建模，因为地图较大的情况下，体素图会占据大量的空间，不利于存储，因此八叉树地图适合做全局地图建模，用于做全局规划
体素图因为数据结构简单，建模速度快，因此适合做局部小地图的建模，用于局部规划，做动态避障。
八叉树代码实现
以下是使用matlab编写的一个八叉树的实现
静态创建八叉树(点云固定空间大小)
主循环文件：
%*************************************************************************%%使用递归的方式根据真实点云数据静态创建八叉树(地图固定，不会越界)
%**********************************************************************%
clc;
clear;
%**********读取点云数据并进行下采样**********%
figure
filename = 'cloud.pcd';
pointCloud = pcread(filename);
% 下采样参数设置
gridSize = 2; % 下采样网格大小，根据需要进行调整 数值越大，砍掉的点越多
% 执行下采样
downsampledPtCloud = pcdownsample(pointCloud, 'gridAverage', gridSize);
[p_num, n] = size(downsampledPtCloud.Location);
pcshow(pointCloud);
% %显示下采样地图
% figure
% pcshow(downsampledPtCloud);
%****************建立八叉树**************%
%计算八叉树的根节点需要用到的参数
x_min = min(downsampledPtCloud.Location(:, 1));
y_min = min(downsampledPtCloud.Location(:, 2));
z_min = min(downsampledPtCloud.Location(:, 3));
x_max = max(downsampledPtCloud.Location(:, 1));
y_max = max(downsampledPtCloud.Location(:, 2));
z_max = max(downsampledPtCloud.Location(:, 3));
%以xyz点中最大最小值差异最大的作为边长
x_side = (x_max - x_min);
y_side = (y_max - y_min);
z_side = (z_max - z_min);
side = [x_side y_side z_side];
side_len = max(side) + 1;
%创建八叉树图
figure
xlabel('x');
ylabel('y');
zlabel('z');
view(3); %设定为三维视角
Root = struct(); %创建一个空结构体
Root.isLeaf = false; %是否是叶子节点 (叶子节点可能是障碍物，也可能不是)
Root.isOccupiedLeaf = false; %是否是占据叶子
Root.value = 1; %1表示有点云数据，可以继续分割
Root.index = 1; %bitshift表示移位 编号，每一位分为8个节点，每一位的编号代表其位置
Root.x = x_min + x_side / 2; %此节点方格的中心坐标
Root.y = y_min + y_side / 2;
Root.z = z_min + z_side / 2;
Root.level = 0; %当前层数 根节点为第0层
Root.side_len = side_len; %边长
Root.children = {}; %子节点
%***************根据精度创建八叉树(静态创建)********************%
%根据边长与方块边长的要求，计算需要创建多少深度的八叉树
n = 0;
l = side_len;
while l > 5l = l / 2;n = n + 1;
end
resolution_ratio = n; %分辨率 向下分n层
obs_leaf = [];
obs_leaf_num = 0;
[Octree, obs_leaf, obs_leaf_num] = create_octree_static(Root, downsampledPtCloud.Location, p_num, resolution_ratio, 1, obs_leaf, obs_leaf_num);
%绘制八叉树地图
draw_obs_leaf_node(obs_leaf, obs_leaf_num, Root.x, Root.y, Root.z, side_len, resolution_ratio, z_min, z_max);递归创建八叉树函数：
%*************************************************************************%%递归创建八叉树函数 任意层%输出：node--当前节点 p--点云数据% num--点云数量 level_max--最大层数% obs_leaf_index--存储叶子节点的编号% obs_leaf_num--叶子节点数量
%**********************************************************************%
% 创建八叉树函数
function [node, obs_leaf_index, obs_leaf_num] = create_octree_static(node, p, num, level_max, level_current, obs_leaf_index, obs_leaf_num)%若节点为空，则返回失败，根节点不能为空if isempty(node)return;%当前不是最大层级elseif level_current <= level_max%判断节点是否未被占据，即没有数据点在方块内if isNullPoint(node, p, num)%当前是有数据的叶子节点，并且当前层级为最大层级时，标记节点为占据叶子节点node.isLeaf = true; node.isOccupiedLeaf = false;node.value = 1; node.children = {};else %非叶子节点 可以继续往下分割node.isLeaf = false; node.isOccupiedLeaf = false;node.value = 1;node.children = {}; %节点创建八个子节点%创建根节点的八个子节点 此为第一层 根节点为第0层node.children = create_sub_node(node, p, num, level_current, level_max); %创建8个子节点 层级为1%递归创建树for i = 1:8[node.children{i}, obs_leaf_index, obs_leaf_num] = create_octree_static(node.children{i}, p, num, level_max, level_current+1, obs_leaf_index, obs_leaf_num);endendelseif level_current > level_max%判断节点是否被占据if isNullPoint(node, p, num)%当前是被占据的叶子节点，并且当前层级为最大层级时，标记节点为障碍物叶子节点node.isLeaf = true; node.isOccupiedLeaf = true;node.value = 1; node.children = {}; obs_leaf_num = obs_leaf_num + 1;obs_leaf_index(obs_leaf_num) = node.index;elsenode.isLeaf = true; node.isOccupiedLeaf = false;node.value = 0; node.children = {}; endreturn;end
end判断是否是未被占据的节点：
%*************************************************************************%%判断节点是否未被占据，即空的
%**********************************************************************%
% 判断是否满足叶节点条件 node--节点 p--数据点集 num--数据点集数量
function result = isNullPoint(node, p, num)result = true;%检测节点的方块内是否有数据点for i = 1:numif (p(i, 1) < (node.x + (node.side_len / 2))) && ...(p(i, 1) >= (node.x - (node.side_len / 2))) && ...(p(i, 2) < (node.y + (node.side_len / 2))) && ...(p(i, 2) >= (node.y - (node.side_len / 2))) && ...(p(i, 3) < (node.z + (node.side_len / 2))) && ...(p(i, 3) >= (node.z - (node.side_len / 2)))result = false;break; %检测到有便可以退出endend
end绘图函数：
%*************************************************************************%%绘制叶子节点 %输入：index--叶子节点编号 index_num--叶子节点数量% root_x,root_y,root_z--根节点坐标 root_side_len--根节点边长(最大的方块边长)% level--八叉树深度 min_z--最小高度 max_z--最大高度(用于根据高度渐变颜色)% 注意：画图时，需要计算方块的左下前坐标(即8个顶点中，各坐标值最小的)
%**********************************************************************%
%******************%
% 以最大的方块为第一层的根
% 编号规则(顺时针)：
% 底层: 2 3
% 1 4
% 顶层: 6 7
% 5 8
%********************%
function draw_obs_leaf_node(index, index_num, root_x, root_y, root_z, root_side_len, level, z_min, z_max)%计算边长for i = 1:index_num%根据编号计算坐标;index_current = floor(index(i) / 10); %第一层为根节点，可以忽略x = root_x;y = root_y;z = root_z;side_len = root_side_len;for j = 1:levelindex_level = mod(index_current, 10); % 取余side_len = side_len/2;switch index_levelcase 1x = x - side_len / 2;y = y - side_len / 2;z = z - side_len / 2;case 2x = x - side_len / 2;y = y + side_len / 2;z = z - side_len / 2;case 3x = x + side_len / 2;y = y + side_len / 2;z = z - side_len / 2;case 4x = x + side_len / 2;y = y - side_len / 2;z = z - side_len / 2;case 5x = x - side_len / 2;y = y - side_len / 2;z = z + side_len / 2;case 6x = x - side_len / 2;y = y + side_len / 2;z = z + side_len / 2;case 7x = x + side_len / 2;y = y + side_len / 2;z = z + side_len / 2;case 8x = x + side_len / 2;y = y - side_len / 2;z = z + side_len / 2;endindex_current = floor(index_current / 10);end%计算最终画图的坐标draw_x = x - side_len/2;draw_y = y - side_len/2;draw_z = z - side_len/2;%根据不同的高度绘制不同的颜色cmap = jet; % 使用 jet 色图，可以根据需要选择其他色图z_normalized = (z - z_min) / (z_max - z_min); % 将高度归一化到 [0, 1] 范围color = ind2rgb(round(z_normalized * (size(cmap, 1) - 1)) + 1, cmap); % 将归一化的高度值映射到颜色映射中draw_cube(draw_x, draw_y, draw_z, side_len, color);end
end之后是采集了一些点云来进行了验证，因为大小是固定的，因此是静态创建八叉树的效果：
