Оснуйте обнаружение плоскости и препятствия Используя лазерный дальномер
Этот пример показывает, как обработать 3-D данные о Лазерном дальномере от датчика, смонтированного на автомобиле путем сегментации наземной плоскости и нахождения соседних препятствий. Это может упростить drivable планирование пути для навигации автомобиля. Пример также показывает, как визуализировать данные о Лазерном дальномере потоковой передачи.
Создайте VelodyneFileReader
Данные о Лазерном дальномере, используемые в этом примере, были зарегистрированы с помощью датчика Velodyne HDL32E, смонтированного на автомобиле. Setup объект 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 double]
Color: []
Normal: []
Intensity: [32×1083 double]
Count: 34656
XLimits: [-80.0444 87.1780]
YLimits: [-85.6287 92.8721]
ZLimits: [-21.6060 14.3558]
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
