The 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)