occupancyMap3D препятствия объектно-ориентированной памяти в трехмерном пространстве, с помощью наблюдений датчика, чтобы сопоставить среду. Создайте карту и добавьте точки из облака точек, чтобы идентифицировать препятствия. Затем раздуйте препятствия в карте, чтобы гарантировать безопасный операционный пробел вокруг препятствий.

Создайте occupancyMap3D объект с разрешением карты 10 ячеек/метр.

map3D = occupancyMap3D(10);

Задайте набор 3-D точек как наблюдение от положения [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)