八叉樹與體素圖
八叉樹地圖
八叉樹地圖是可變分辨率的三維柵格地圖，可以自由調整分辨率，如下所示：
根據點云的數量或密度決定每個葉子方塊是否被占據
體素圖
體素就是固定分辨率的三維柵格地圖，如下所示：
根據點云的數量或密度決定每個體素是否被占據
八叉樹地圖與體素圖的區別
最大的區別是八叉樹地圖是可變分辨率，體素圖是固定分辨率，也就是說，在一定大小的三維空間內，體素圖是按照最小的分割體積，將空間完整分割表達，數據結構簡單，不過存儲量更多一些，但是建圖速度更快。
八叉樹地圖使用八叉樹的數據結構來存儲，對于一些沒有點云或點云數量極少的空區域，可以不進行展開分割，只存儲這一大塊體積，而對點云有效，密集的區域進行進一步的分割存儲，也就是所說的可變分辨率，這樣做減少存儲量，但數據結構復雜，通常情況下建圖速度沒有體素圖快
八叉樹地圖適合做大地圖的建模，因為地圖較大的情況下，體素圖會占據大量的空間，不利于存儲，因此八叉樹地圖適合做全局地圖建模，用于做全局規劃
體素圖因為數據結構簡單，建模速度快，因此適合做局部小地圖的建模，用于局部規劃，做動態避障。
八叉樹代碼實現
以下是使用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之后是采集了一些點云來進行了驗證，因為大小是固定的，因此是靜態創建八叉樹的效果：
)
)
