Оснуйте обнаружение плоскости и препятствия Используя лидар

Открыть скрипт

В этом примере показано, как обработать 3-D данные о лидаре от датчика, смонтированного на транспортном средстве путем сегментации наземной плоскости и нахождения соседних препятствий. Это может упростить drivable планирование пути для навигации транспортного средства. Пример также показывает, как визуализировать данные о лидаре потоковой передачи.

Создайте средство чтения файлов Velodyne

Данные о лидаре, используемые в этом примере, были зарегистрированы с помощью датчика Velodyne HDL32E, смонтированного на транспортном средстве. Настройте velodyneFileReader возразите, чтобы считать записанный файл PCAP.

fileName    = 'lidarData_ConstructionRoad.pcap';
deviceModel = 'HDL32E';

veloReader = velodyneFileReader(fileName, deviceModel);

Считайте скан лидара

Каждый скан данных о лидаре хранится как 3-D облако точек. Эффективно обработка этих данных, использующих быструю индексацию и поиск, является ключевой для эффективности конвейера обработки датчика. Этот КПД достигается с помощью pointCloud объект, который внутренне организует данные с помощью древовидной структуры данных K-d.

veloReader создает организованный pointCloud для каждого скана лидара. Location свойство pointCloud M N 3 матрицами, содержа координаты XYZ точек в метрах. Интенсивность точки хранится в Intensity.

% Read a scan of lidar data
ptCloud = readFrame(veloReader) %#ok<NOPTS>

ptCloud = 

  pointCloud with properties:

     Location: [32×1083×3 single]
        Count: 34656
      XLimits: [-80.0444 87.1780]
      YLimits: [-85.6287 92.8721]
      ZLimits: [-21.6060 14.3558]
        Color: []
       Normal: []
    Intensity: [32×1083 uint8]

Setup, передающий отображение облака точек потоком

pcplayer может использоваться, чтобы визуализировать данные об облаке точек потоковой передачи. Setup область вокруг транспортного средства, чтобы отобразиться путем конфигурирования pcplayer.

% Specify limits of point cloud display
xlimits = [-25 45]; % meters
ylimits = [-25 45];
zlimits = [-20 20];

% Create a pcplayer
lidarViewer = pcplayer(xlimits, ylimits, zlimits);

% Customize player axes labels
xlabel(lidarViewer.Axes, 'X (m)')
ylabel(lidarViewer.Axes, 'Y (m)')
zlabel(lidarViewer.Axes, 'Z (m)')

% Display the raw lidar scan
view(lidarViewer, ptCloud)

В этом примере мы будем сегментировать точки, принадлежащие наземной плоскости, автомобилю, оборудованному датчиком и соседним препятствиям. Установите палитру для маркировки этих точек.

% Define labels to use for segmented points
colorLabels = [...
    0      0.4470 0.7410; ... % Unlabeled points, specified as [R,G,B]
    0.4660 0.6740 0.1880; ... % Ground points
    0.9290 0.6940 0.1250; ... % Ego points
    0.6350 0.0780 0.1840];    % Obstacle points

% Define indices for each label
colors.Unlabeled = 1;
colors.Ground    = 2;
colors.Ego       = 3;
colors.Obstacle  = 4;

% Set the colormap
colormap(lidarViewer.Axes, colorLabels)

Сегментируйте автомобиль, оборудованный датчиком

Лидар смонтирован сверху транспортного средства, и облако точек может содержать точки, принадлежащие самому транспортному средству, такой как на крыше или капоте. Зная размерности транспортного средства, мы можем сегментировать точки, которые являются самыми близкими к транспортному средству.

Создайте vehicleDimensions объект для хранения размерностей транспортного средства.

vehicleDims = vehicleDimensions(); % Typical vehicle 4.7m by 1.8m by 1.4m

Задайте монтирующееся местоположение лидара в системе координат транспортного средства. Система координат транспортного средства сосредоточена в центре задней оси, на земле, с положительным обращением направления X прямой, положительный Y влево и положительный Z вверх. В этом примере лидар смонтирован на лучшем центре транспортного средства, параллельного земле.

mountLocation = [...
    vehicleDims.Length/2 - vehicleDims.RearOverhang, ... % x
    0, ...                                               % y
    vehicleDims.Height];                                 % z

Сегментируйте автомобиль, оборудованный датчиком с помощью функции помощника helperSegmentEgoFromLidarData. Этот функциональные сегменты все точки в кубоиде заданы автомобилем, оборудованным датчиком. Сохраните сегментированные точки в struct points.

points = struct();
points.EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);

Визуализируйте облако точек с сегментированным автомобилем, оборудованным датчиком. Используйте helperUpdateView функция помощника.

closePlayer = false;
helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

Земля сегмента плоские и соседние препятствия

Для того, чтобы идентифицировать препятствия из данных о лидаре, первый сегмент наземная плоскость использование segmentGroundFromLidarData функция, чтобы выполнить это. Этот функциональные точки сегментов, принадлежащие земле из подготовленных лидарных данных.

elevationDelta = 10;
points.GroundPoints = segmentGroundFromLidarData(ptCloud, 'ElevationAngleDelta', elevationDelta);

% Visualize the segmented ground plane.
helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

Удалите точки, принадлежащие автомобилю, оборудованному датчиком и наземной плоскости при помощи select функция на облаке точек. Задайте 'OutputSize' как 'full' сохранить организованную природу облака точек.

nonEgoGroundPoints = ~points.EgoPoints & ~points.GroundPoints;
ptCloudSegmented = select(ptCloud, nonEgoGroundPoints, 'OutputSize', 'full');

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

sensorLocation  = [0, 0, 0]; % Sensor is at the center of the coordinate system
radius          = 40; % meters

points.ObstaclePoints = findNeighborsInRadius(ptCloudSegmented, ...
    sensorLocation, radius);

% Visualize the segmented obstacles
helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);

Последовательность лидара процесса

Теперь, когда конвейер обработки облака точек для одного скана лидара был размечен, помещал это все вместе, чтобы обработать 30 секунд от последовательности записанных данных. Код ниже сокращен, поскольку основные параметры были заданы на предыдущих шагах. Здесь, параметры используются без дальнейшего объяснения.

% Rewind the |veloReader| to start from the beginning of the sequence
reset(veloReader);

% Stop processing after 30 seconds
stopTime = veloReader.StartTime + seconds(30);

isPlayerOpen = true;
while hasFrame(veloReader) && veloReader.CurrentTime < stopTime && isPlayerOpen

    % Grab the next lidar scan
    ptCloud = readFrame(veloReader);

    % Segment points belonging to the ego vehicle
    points.EgoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation);

    % Segment points belonging to the ground plane
    points.GroundPoints = segmentGroundFromLidarData(ptCloud, 'ElevationAngleDelta', elevationDelta);

    % Remove points belonging to the ego vehicle and ground plane
    nonEgoGroundPoints = ~points.EgoPoints & ~points.GroundPoints;
    ptCloudSegmented = select(ptCloud, nonEgoGroundPoints, 'OutputSize', 'full');

    % Segment obstacles
    points.ObstaclePoints = findNeighborsInRadius(ptCloudSegmented, sensorLocation, radius);

    closePlayer = ~hasFrame(veloReader);

    % Update lidar display
    isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer);
end
snapnow

Вспомогательные Функции

helperSegmentEgoFromLidarData точки сегментов, принадлежащие автомобилю, оборудованному датчиком, учитывая размерности транспортного средства и монтирующие местоположение.

function egoPoints = helperSegmentEgoFromLidarData(ptCloud, vehicleDims, mountLocation)
%helperSegmentEgoFromLidarData segment ego vehicle points from lidar data
%   egoPoints = helperSegmentEgoFromLidarData(ptCloud,vehicleDims,mountLocation)
%   segments points belonging to the ego vehicle of dimensions vehicleDims
%   from the lidar scan ptCloud. The lidar is mounted at location specified
%   by mountLocation in the vehicle coordinate system. ptCloud is a
%   pointCloud object. vehicleDimensions is a vehicleDimensions object.
%   mountLocation is a 3-element vector specifying XYZ location of the
%   lidar in the vehicle coordinate system.
%
%   This function assumes that the lidar is mounted parallel to the ground
%   plane, with positive X direction pointing ahead of the vehicle,
%   positive Y direction pointing to the left of the vehicle in a
%   right-handed system.

% Buffer around ego vehicle
bufferZone = [0.1, 0.1, 0.1]; % in meters

% Define ego vehicle limits in vehicle coordinates
egoXMin = -vehicleDims.RearOverhang - bufferZone(1);
egoXMax = egoXMin + vehicleDims.Length + bufferZone(1);
egoYMin = -vehicleDims.Width/2 - bufferZone(2);
egoYMax = egoYMin + vehicleDims.Width + bufferZone(2);
egoZMin = 0 - bufferZone(3);
egoZMax = egoZMin + vehicleDims.Height + bufferZone(3);

egoXLimits = [egoXMin, egoXMax];
egoYLimits = [egoYMin, egoYMax];
egoZLimits = [egoZMin, egoZMax];

% Transform to lidar coordinates
egoXLimits = egoXLimits - mountLocation(1);
egoYLimits = egoYLimits - mountLocation(2);
egoZLimits = egoZLimits - mountLocation(3);

% Use logical indexing to select points inside ego vehicle cube
egoPoints = ptCloud.Location(:,:,1) > egoXLimits(1) ...
    & ptCloud.Location(:,:,1) < egoXLimits(2) ...
    & ptCloud.Location(:,:,2) > egoYLimits(1) ...
    & ptCloud.Location(:,:,2) < egoYLimits(2) ...
    & ptCloud.Location(:,:,3) > egoZLimits(1) ...
    & ptCloud.Location(:,:,3) < egoZLimits(2);
end

helperUpdateView обновляет отображение облака точек потоковой передачи с последним облаком точек и сопоставленными цветными метками.

function isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayer)
%helperUpdateView update streaming point cloud display
%   isPlayerOpen = helperUpdateView(lidarViewer, ptCloud, points, colors, closePlayers)
%   updates the pcplayer object specified in lidarViewer with a new point
%   cloud ptCloud. Points specified in the struct points are colored
%   according to the colormap of lidarViewer using the labels specified by
%   the struct colors. closePlayer is a flag indicating whether to close
%   the lidarViewer.

if closePlayer
    hide(lidarViewer);
    isPlayerOpen = false;
    return;
end

scanSize = size(ptCloud.Location);
scanSize = scanSize(1:2);

% Initialize colormap
colormapValues = ones(scanSize, 'like', ptCloud.Location) * colors.Unlabeled;

if isfield(points, 'GroundPoints')
    colormapValues(points.GroundPoints) = colors.Ground;
end

if isfield(points, 'EgoPoints')
    colormapValues(points.EgoPoints) = colors.Ego;
end

if isfield(points, 'ObstaclePoints')
    colormapValues(points.ObstaclePoints) = colors.Obstacle;
end

% Update view
view(lidarViewer, ptCloud.Location, colormapValues)

% Check if player is open
isPlayerOpen = isOpen(lidarViewer);

end

Смотрите также

Функции

Объекты

Похожие темы

Документация Automated Driving Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте