Раздуйте карту
inflate( раздувает каждое занятое положение заданного во входе map3D,radius)occupancyMap3D объект radius заданный в метрах. radius окружен к самой близкой эквивалентной ячейке на основе разрешения карты. Эта инфляция увеличивает размер занятых местоположений в карте.
Импортируйте 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
