Обнаружение наземной плоскости и препятствий с помощью лидара
Этот пример показывает, как обработать данные 3-D лидара с датчика, установленного на транспортном средстве, сегментировав наземную плоскость и обнаружив близлежащие препятствия. Это может облегчить планирование приводимого в действие пути для навигации по транспортному средству. Пример также показывает, как визуализировать потоковые данные лидара.
Создайте средство чтения файлов Velodyne
Данные лидара, используемые в этом примере, регистрировали с помощью датчика Velodyne HDL32E, установленного на транспортном средстве. Настройка velodyneFileReader
fileName = 'lidarData_ConstructionRoad.pcap'; deviceModel = 'HDL32E'; veloReader = velodyneFileReader(fileName, deviceModel);
Чтение скана лидара
Каждый скан данных лидара хранится как 3-D облако точек. Эффективная обработка этих данных с помощью быстрой индексации и поиска является ключом к эффективности конвейера обработки датчика. Эта эффективность достигается с помощью pointCloud
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
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
