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