Вычислите индексы ячейки вдоль луча
[ возвращает индексы ячейки заданного endpoints,midpoints]
= raycast(map,pose,range,angle)map для всех ячеек, пересеченных лучом, происходящим из заданного pose в заданном angle и range значения. endpoints содержит все индексы, затронутые к концу луча, со всеми другими точками, включенными в midpoints.
Используйте raycast метод, чтобы сгенерировать индексы ячейки для всех ячеек, пересеченных лучом.
Создайте пустую карту. Карта с низкой разрешающей способностью используется, чтобы проиллюстрировать произведенные местоположения сетки.
map = occupancyMap(10,10,1); show(map)
Получите индексы сетки средних точек и конечные точки луча от [2 3] к [8.5 8]. Установите значения заполнения для этих индексов сетки. Средние точки обработаны как открытое пространство. Обновите конечные точки с занятым наблюдением.
p1 = [2 3]; p2 = [8.5 8]; [endPts,midPts] = raycast(map,p1,p2); setOccupancy(map,midPts,zeros(length(midPts),1),'grid'); setOccupancy(map,endPts,ones(length(endPts),1),'grid');
Постройте исходный луч по карте. Каждая ячейка сетки, затронутая линией, обновляется. Начальная точка перекрывает несколько ячеек, и линия касается ребра определенных ячеек, но все ячейки все еще обновляются.
show(map) hold on plot([p1(1) p2(1)],[p1(2) p2(2)],'-b','LineWidth',2) plot(p2(1),p2(2),'or') grid on
