创建三维占用图
描述
occupancyMap3D 对象存储三维地图和地图信息。地图以概率值的形式存储在一个八叉树数据结构中。该类可处理任意环境，并根据观测输入动态扩展其大小。您可以以点云或特定 xyz 位置的形式添加观测数据。这些观察结果会更新概率值。概率值表示位置的占用率。八叉树数据结构会对数据进行适当修剪，以保持在内存和磁盘中的效率。
创造
语法
omap = occupancyMap3D(res,Name,Value)
描述
omap = occupancyMap3D 创建一个没有观测值和默认属性值的空三维占用图。
omap = occupancyMap3D(res) 以单元/米为单位指定地图分辨率并设置分辨率属性。
omap = occupancyMap3D(res,Name,Value) 使用一个或多个名值参数指定属性。例如，"FreeThreshold",0.25 设置的阈值为 0.25，即认为单元格无障碍的概率值。
属性
Resolution - 网格分辨率
1（默认值） | 正标量
网格分辨率（以每米一个单元为单位），以标量形式指定。在构建时指定分辨率。插入精度高于此值的观测数据时，将按此分辨率向下舍入并应用。
FreeThreshold - 将单元格视为无障碍单元格的阈值
0.2（默认值） | 正标量
将单元格视为无障碍单元格的阈值，以正标量形式指定。低于此阈值的概率值被视为无障碍。
OccupiedThreshold - 将单元格视为被占用的阈值
0.65（默认值） | 正标量
将单元格视为已占用的阈值，以正标量形式指定。超过此阈值的概率值将被视为被占用。
ProbabilitySaturation - 概率值的饱和度限制
[0.001 0.999]（默认） | [min max] 向量
概率值的饱和限制，以 [min max] 向量形式指定。高于或低于这些饱和值的值将被设置为最小值或最大值。该属性可在包含多个观测值时减少单元格的过饱和。
对象函数
| checkOccupancy | 检查位置是否空闲或被占用 |
| getOccupancy | 获取地点的占用概率 |
| inflate | 为每个被占用的位置膨胀 |
| insertPointCloud | 将三维点或点云观测数据插入地图 |
| rayIntersection | 查找射线与所占地图单元的交点 |
| setOccupancy | 设置地点的占用概率 |
| show | 显示 3-D 占用地图 |
| updateOccupancy | 更新各地点的占用概率 |
创建三维占位图和膨胀点
% occupancyMap3D 对象利用传感器观测数据绘制环境地图，在三维空间中存储障碍物。
% 创建地图并添加点云中的点以识别障碍物。
% 然后在地图中放大障碍物，确保障碍物周围有安全的操作空间
map3D = occupancyMap3D(10);%创建一个 occupancyMap3D 对象，地图分辨率为 10 个单元/米
% 将一组三维点定义为从一个姿态 [x y z qw qx qy qz] 观察到的点。
% 该姿态是观测这些点的传感器的姿态，以原点为中心。定义两组点以插入多个观测点
pose = [ 0 0 0 1 0 0 0];
points = repmat((0:0.25:2)', 1, 3);
points2 = [(0:0.25:2)' (2:-0.25:0)' (0:0.25:2)'];
maxRange = 5;
% 使用 insertPointCloud 插入第一组点
% 该函数使用传感器姿态和给定点将观测数据插入地图。显示的颜色与点的高度相关，仅供参考
insertPointCloud(map3D,pose,points,maxRange)
show(map3D)
% 插入第二组点。传感器姿态（原点）与这些点之间的射线与之前插入的点重叠。
% 因此，传感器和新点之间的自由空间会被更新并标记为自由空间。
insertPointCloud(map3D,pose,points2,maxRange)
show(map3D)
% 为地图膨胀，增加一个缓冲区，以便在障碍物周围安全操作。
% 定义车辆半径和安全距离，并使用这些值的总和来定义地图的膨胀半径。
vehicleRadius = 0.2; %车辆半径
safetyRadius = 0.3; %安全距离
inflationRadius = vehicleRadius + safetyRadius; %膨胀半径
inflate(map3D, inflationRadius);
show(map3D)
八叉树数据结构
八叉树数据结构是一种分层结构，用于将环境细分为称为体素的立方体。对于给定的地图体积，空间会递归细分为八个体素，直到达到所需的地图分辨率（体素大小）为止。这种细分可表示为一棵树，树中存储了地图中各个位置的概率值。
树中的概率值采用对数表示法。这种表示法能使位置从动态观测中轻松恢复，并减少因概率小而产生的数值误差。为了保持内存效率，如果树的下部分支使用这种对数表示法共享相同的占用值，则会在结构上对这些分支进行剪枝。
对象在内部处理该数据结构的组织，包括修剪分支。在使用 setOccupancy、getOccupancy 或 insertPointCloud 等函数时，请将所有观测值指定为空间坐标。该对象会根据空间坐标和地图的分辨率来决定是否插入到树中以及在树中的导航。
参考资料
[1] Hornung, Armin, Kai M. Wurm, Maren Bennewitz, Cyrill Stachniss, and Wolfram Burgard. "OctoMap: an efficient probabilistic 3D mapping framework based on octrees." Autonomous Robots 34, no. 3 (April 2013): 189–206. https://doi.org/10.1007/s10514-012-9321-0.
checkOccupancy
检查位置是否空闲或被占用
语法
iOccval = checkOccupancy(map3D,xyz)
描述
iOccval = checkOccupancy(map3D,xyz) 使用输入 occupancyMap3D 对象的 OccupiedThreshold 和 FreeThreshold 属性，返回在 xyz 位置指定的占用率值数组。每一行都是地图中要检查占用率的单独 xyz 位置。占用值可以是无障碍（0）、占用（1）或未知（-1）。
map3D = importOccupancyMap3D("citymap.ot") %导入三维占位图
show(map3D) %显示地图
% 检查不同地点的占用状态并获取占用值。
iOccVal1 = checkOccupancy(map3D,[50 15 0])
OccVal1 = getOccupancy (map3D,[50 15 0])
iOccVal2 = checkOccupancy(map3D,[50 15 15])
OccVal2 = getOccupancy (map3D,[50 15 15])
iOccVal3 = checkOccupancy(map3D,[50 15 45])
OccVal3 = getOccupancy (map3D,[50 15 45])
输入参数
map3D - 3-D 占位图
occupancyMap3D 对象
三维空间占用图，指定为 occupancyMap3D 对象。
xyz - 世界坐标
n-by-3 矩阵
世界坐标，指定为 [x y z] 点的 n-by-3 矩阵，其中 n 是世界坐标的个数。
输出参数
iOccval - 占用率解释值
列向量
占空比解释值，以列向量形式返回，长度与 xyz 相同。
占用值可以是无障碍（0）、占用（1）或未知（-1）。这些值是根据实际概率值以及 map3D 对象的 OccupiedThreshold 和 FreeThreshold 属性确定的。
getOccupancy
获取地点的占用概率
inflate
为每个被占用的位置膨胀
语法
描述
inflate(map3D,radius)会将输入的 occupancyMap3D 对象中指定的每个占用位置按指定的半径（以米为单位）膨胀。这种膨胀会增加地图中被占用位置的大小。
输入参数
map3D - 3-D 占位图
occupancyMap3D 对象
三维空间占用图，指定为 occupancyMap3D 对象。
radius - 膨胀所占位置的数量
标量
以标量形式指定的占用位置膨胀量。半径四舍五入为最接近的单元格值。
insertPointCloud
将三维点或点云观测数据插入地图
语法
insertPointCloud(map3D,pose,points,maxrange)
insertPointCloud(map3D,pose,ptcloud,maxrange)
描述
insertPointCloud(map3D,pose,points,maxrange)（插入点云）在占用图 map3D 的给定点上插入一个或多个传感器观测值。占用点的观测值将更新为 0.7。传感器姿态和点之间的所有其他点都被视为无障碍点，并以 0.4 的观测值进行更新。超出最大范围的点不会更新。NaN 值将被忽略。
insertPointCloud(map3D,pose,ptcloud,maxrange) 在地图中插入一个 ptcloud 对象。
输入参数
map3D - 3-D 占位图
occupancyMap3D 对象
三维空间占用图，指定为 occupancyMap3D 对象。
points - 点云的点
n-by-3 矩阵
以传感器坐标表示的点云的点，指定为一个 n-by-3 的 [x y z] 点矩阵，其中 n 是点云中的点数。
ptcloud - 读取点云
点云对象
点云读数，指定为 pointCloud 对象。
注释
使用点云对象需要使用 Computer Vision Toolbox™。
pose - 车辆的位置和方向
[x y z qw qx qy qz] 矢量
车辆的位置和方向，指定为[x y z qw qx qy qz]向量。车辆姿态是一个 xyz 位置向量，其四元数方向向量指定为 [qw qx qy qz]。
maxrange - 传感器的最大量程
标量
点云传感器的最大范围，以标量形式指定。超出此范围的点将被忽略。
rayIntersection
查找射线与所占地图单元的交点
语法
[intersectionPts,isOccupied] = rayIntersection(map3D,sensorPose,directions,maxrange)
[intersectionPts,isOccupied] = rayIntersection(map3D,sensorPose,directions,maxrange,ignoreUnknown)
描述
[intersectionPts,isOccupied] = rayIntersection(map3D,sensorPose,directions,maxrange) 返回指定地图 map3D 中射线的交点。射线以给定的方向从指定的 sensorPose 射出。交点以世界坐标框架返回。使用 isOccupied 来确定交点是否在传感器最大范围内或是否与障碍物相交。
[intersectionPts,isOccupied] = rayIntersection(map3D,sensorPose,directions,maxrange,ignoreUnknown)还接受传感器最大范围和是否忽略未知值的可选参数。默认情况下，射线会延伸到地图边界，并忽略未知值。
输入参数
map3D - 3-D 占位图
occupancyMap3D 对象
三维空间占用图，指定为 occupancyMap3D 对象。
sensorPose - 传感器的位置和方向
[x y z qw qx qy qz] 矢量
传感器的位置和方向，指定为[x y z qw qx qy qz]向量。车辆姿态是一个 xyz 位置向量，其四元数方向向量指定为 [qw qx qy qz]。
directions - 从传感器发出的射线的方向
n-by-3 [dx dy dz] 矩阵 | n-by-2 [az el] 矩阵
传感器发出的射线相对于传感器坐标系的方向，以 n-by-3 [dx dy dz] 矩阵或 n-by-2 [az el] 矩阵形式指定。
[dx dy dz] 是 xyz 坐标的方向向量。
[az el]是一个矢量，其方位角 az 从正 x 方向量度到正 y 方向，仰角从 xy 平面量度到传感器坐标框架中的正 z 方向。
maxrange - 传感器的最大量程
标量
激光测距传感器的最大范围，以米为单位的标量。大于或等于 maxrange 的测距值被认为是整个射线长度上的自由值，直至 maxrange。
ignoreUnknown - 将未知值解释为空闲或占用
1（默认） | 0
将地图中的未知值解释为空闲或占用，指定值为 1 或 0。 设置该值为 0 时，假定未知值被占用。
输出参数
intersectionPts - 相交点
n-by-3 矩阵
交叉点，以世界帧中 [x y z] 点的 n-by-3 矩阵形式返回，其中 n 是方向的长度。
isOccupied - 射线端点的占用状态
0 和 1 的向量
射线端点的占用状态，以 0 和 1 的向量形式返回。使用 isOccupied 可以确定交点是否在传感器最大范围内，或者是否与障碍物相交。
setOccupancy
设置地点的占用概率
语法
setOccupancy(map3D,xyz,occval)
描述
setOccupancy(map3D,xyz,occval)为三维占位图中每个指定的 xyz 坐标分配占位值。
例子
% 创建并导出三维占位图
map3D = occupancyMap3D; %创建 occupancyMap3D 对象
% 创建一个地平面，并将占用值设置为 0
[xGround,yGround,zGround] = meshgrid(0:100,0:100,0);
xyzGround = [xGround(:) yGround(:) zGround(:)];
occval = 0;
setOccupancy(map3D,xyzGround,occval)
% 在地图上的特定世界位置设置障碍
[xBuilding1,yBuilding1,zBuilding1] = meshgrid(20:30,50:60,0:30);
[xBuilding2,yBuilding2,zBuilding2] = meshgrid(50:60,10:30,0:40);
[xBuilding3,yBuilding3,zBuilding3] = meshgrid(40:60,50:60,0:50);
[xBuilding4,yBuilding4,zBuilding4] = meshgrid(70:80,35:45,0:60);
xyzBuildings = [xBuilding1(:) yBuilding1(:) zBuilding1(:);...
xBuilding2(:) yBuilding2(:) zBuilding2(:);...
xBuilding3(:) yBuilding3(:) zBuilding3(:);...
xBuilding4(:) yBuilding4(:) zBuilding4(:)];
% 用新的概率值更新障碍物并显示地图
obs = 0.65;
updateOccupancy(map3D,xyzBuildings,obs)
show(map3D)
% 检查当前目录中是否已存在名为 citymap.ot 的地图文件，并在创建地图文件前将其删除
if exist("citymap.ot",'file')
delete("citymap.ot")
end
% 将地图导出为八叉树文件
filePath = fullfile(pwd,"citymap.ot");
exportOccupancyMap3D(map3D,filePath)输入参数
map3D - 3-D 占位图
occupancyMap3D 对象
三维空间占用图，指定为 occupancyMap3D 对象。
xyz - 世界坐标
n-by-3 矩阵
世界坐标，指定为 [x y z] 点的 n-by-3 矩阵，其中 n 是世界坐标的个数。
occval - 概率占用值
标量 | 列向量
概率占用值，指定为与 xyz 相同长度的标量或列向量。标量输入适用于 xyz 中的所有坐标。
接近 0 的值表示单元格肯定未被占用且无障碍。
map3D = importOccupancyMap3D("citymap.ot") %导入三维占位图
show(map3D) %显示地图
inflate(map3D,1) %以1米为半径放大被占区域
show(map3D)
% 找到射线与所占地图单元格的交点
numRays = 10; %射线数量
angles = linspace(-pi/2,pi/2,numRays); %射线角度范围
directions = [cos(angles); sin(angles); zeros(1,numRays)]'; %射线方向
sensorPose = [55 40 1 1 0 0 0]; %传感器位置
maxrange = 15;
[intersectionPts,isOccupied] = rayIntersection(map3D,sensorPose,directions,maxrange)
% 绘制交点，并绘制从姿态到交点的射线
hold on
plotTransforms(sensorPose(1:3),sensorPose(4:end),...
'FrameSize',5,'MeshFilePath','groundvehicle.stl') % 车辆传感器姿态
for i = 1:numRays
plot3([sensorPose(1),intersectionPts(i,1)],...
[sensorPose(2),intersectionPts(i,2)],...
[sensorPose(3),intersectionPts(i,3)],'-b') % 绘制射线
if isOccupied(i) == 1
plot3(intersectionPts(i,1),intersectionPts(i,2),intersectionPts(i,3),'*r') % 交点
end
end 
updateOccupancy
更新各地点的占用概率
updateOccupancy(map3D,xyz,obs) 对 occupancyMap3D 对象 map3D 中每个指定 xyz 坐标的观测值 obs 进行概率积分。
3092
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
