Покажите карту заполнения
axes = show(map3D)map3D, в текущей системе координат, с метками осей, представляющими мировые координаты.
Функция отображает 3-D среду с помощью 3-D вокселей для областей со значениями заполнения, больше, чем OccupiedThreshold значение свойства задано в map3D. Цвет 3-D графика строго основан на высоте.
Импортируйте 3-D карту заполнения.
map3D = importOccupancyMap3D("citymap.ot")map3D =
occupancyMap3D with properties:
ProbabilitySaturation: [1.0000e-03 0.9990]
Resolution: 1
OccupiedThreshold: 0.6500
FreeThreshold: 0.2000
Раздуйте занятые области радиусом 1 м. Отобразите карту.
inflate(map3D,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)
intersectionPts = 10×3
55.0000 32.0000 1.0000
57.9118 32.0000 1.0000
61.7128 32.0000 1.0000
67.9904 32.5000 1.0000
69.0000 37.5314 1.0000
69.0000 42.4686 1.0000
67.9904 47.5000 1.0000
64.6418 51.4907 1.0000
58.2757 49.0000 1.0000
55.0000 49.0000 1.0000
isOccupied = 10×1
1
1
1
-1
1
1
-1
-1
1
1
Постройте точки пересечения и лучи от положения.
hold on plotTransforms(sensorPose(1:3),sensorPose(4:end),... 'FrameSize',5,'MeshFilePath','groundvehicle.stl') % Vehicle sensor pose for i = 1:numRays plot3([sensorPose(1),intersectionPts(i,1)],... [sensorPose(2),intersectionPts(i,2)],... [sensorPose(3),intersectionPts(i,3)],'-b') % Plot rays if isOccupied(i) == 1 plot3(intersectionPts(i,1),intersectionPts(i,2),intersectionPts(i,3),'*r') % Intersection points end end
