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

Этот пример использует:

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

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

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

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

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

veloReader = velodyneFileReader(fileName, deviceModel);

Чтение скана лидара

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

The veloReader создает организованную pointCloud для каждого скана лидара. The Location свойство pointCloud - матрица M-by-N-by-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 отображения потокового облака точек

The pcplayer может использоваться, чтобы визуализировать потоковые данные облака точек. Настройте область вокруг транспортного средства для отображения путем конфигурирования 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 (Automated Driving Toolbox) объект для хранения размерностей транспортного средства.

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

Сегментируйте автомобиль , оборудованный датчиком с помощью функции helper 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

Документация по Computer Vision Toolbox

Поддержка

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