八叉树地图的原理与实现

八叉树与体素图

八叉树地图

八叉树地图是可变分辨率的三维栅格地图，可以自由调整分辨率，如下所示：

根据点云的数量或密度决定每个叶子方块是否被占据

体素图

体素就是固定分辨率的三维栅格地图，如下所示：

根据点云的数量或密度决定每个体素是否被占据

八叉树地图与体素图的区别

最大的区别是八叉树地图是可变分辨率，体素图是固定分辨率，也就是说，在一定大小的三维空间内，体素图是按照最小的分割体积，将空间完整分割表达，数据结构简单，不过存储量更多一些，但是建图速度更快。

八叉树地图使用八叉树的数据结构来存储，对于一些没有点云或点云数量极少的空区域，可以不进行展开分割，只存储这一大块体积，而对点云有效，密集的区域进行进一步的分割存储，也就是所说的可变分辨率，这样做减少存储量，但数据结构复杂，通常情况下建图速度没有体素图快

八叉树地图适合做大地图的建模，因为地图较大的情况下，体素图会占据大量的空间，不利于存储，因此八叉树地图适合做全局地图建模，用于做全局规划

体素图因为数据结构简单，建模速度快，因此适合做局部小地图的建模，用于局部规划，做动态避障。

八叉树代码实现

以下是使用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 > 5
    l = 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);
            end
        end
    elseif 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;
        else
            node.isLeaf = true; 
            node.isOccupiedLeaf = false;
            node.value = 0;  
            node.children = {};             
        end
        return;
    end
end

判断是否是未被占据的节点：

%*************************************************************************%
                %判断节点是否未被占据，即空的
%**********************************************************************%
% 判断是否满足叶节点条件  node--节点   p--数据点集  num--数据点集数量
function result = isNullPoint(node, p, num)
    result = true;
    %检测节点的方块内是否有数据点
    for i = 1:num
        if (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;  %检测到有便可以退出
        end
    end
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:level
            index_level = mod(index_current, 10); % 取余
            side_len = side_len/2;
            switch index_level
                case 1
                    x = x - side_len / 2;
                    y = y - side_len / 2;
                    z = z - side_len / 2;
                case 2
                    x = x - side_len / 2;
                    y = y + side_len / 2;
                    z = z - side_len / 2;
                case 3
                    x = x + side_len / 2;
                    y = y + side_len / 2;
                    z = z - side_len / 2;
                case 4
                    x = x + side_len / 2;
                    y = y - side_len / 2;
                    z = z - side_len / 2;
                case 5
                    x = x - side_len / 2;
                    y = y - side_len / 2;
                    z = z + side_len / 2;
                case 6
                    x = x - side_len / 2;
                    y = y + side_len / 2;
                    z = z + side_len / 2;
                case 7
                    x = x + side_len / 2;
                    y = y + side_len / 2;
                    z = z + side_len / 2;
                case 8
                    x = x + side_len / 2;
                    y = y - side_len / 2;
                    z = z + side_len / 2;
            end
            index_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

之后是采集了一些点云来进行了验证，因为大小是固定的，因此是静态创建八叉树的效果：