В этом примере показано, как обработать 3-D данные о лидаре от датчика, смонтированного на транспортном средстве путем сегментации наземной плоскости и нахождения соседних препятствий. Это может упростить drivable планирование пути для навигации транспортного средства. Пример также показывает, как визуализировать данные о лидаре потоковой передачи.
Данные о лидаре, используемые в этом примере, были зарегистрированы с помощью датчика Velodyne HDL32E, смонтированного на транспортном средстве. Настройте velodyneFileReader
fileName = 'lidarData_ConstructionRoad.pcap'; deviceModel = 'HDL32E'; veloReader = velodyneFileReader(fileName, deviceModel);
Каждое сканирование данных о лидаре хранится как 3-D облако точек. Эффективно обработка этих данных, использующих быструю индексацию и поиск, является ключевой для производительности конвейера обработки датчика. Этот КПД достигается с помощью pointCloud
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]
Color: []
Normal: []
Intensity: [32×1083 single]
Count: 34656
XLimits: [-80.0444 87.1780]
YLimits: [-85.6287 92.8721]
ZLimits: [-21.6060 14.3558]
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
