Вычисление индексов ячеек вдоль луча
[ возвращает индексы ячеек указанного 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
