Этот пример демонстрирует, как обработать 3-D данные о лидаре от датчика, смонтированного на транспортном средстве, чтобы прогрессивно создать карту. Такая карта подходит для автоматизированных ведущих рабочих процессов, таких как локализация и навигация. Эти карты могут использоваться, чтобы локализовать транспортное средство в нескольких сантиметрах.
Существуют различные способы указать облака точек. Типичный подход должен использовать полное облако точек для регистрации. Создайте Карту из Данных о Лидаре (Automated Driving Toolbox), пример использует этот подход для создания карты. Этот пример использует отличительные функции, извлеченные из облака точек для создания карты.
В этом примере вы будете учиться как:
Загрузите и визуализируйте зарегистрированные ведущие данные.
Создайте карту с помощью сканов лидара.
Данные, используемые в этом примере, представляют приблизительно 100 секунд лидара, GPS и данных IMU. Данные сохранены в отдельных MAT-файлах как timetable объекты. Загрузите файл MAT данных о лидаре с репозитория и загрузите его в рабочую область MATLAB®.
Примечание: Эта загрузка может занять несколько минут.
baseDownloadURL = ['https://github.com/mathworks/udacity-self-driving-data' ... '-subset/raw/master/drive_segment_11_18_16/']; dataFolder = fullfile(tempdir, 'drive_segment_11_18_16', filesep); options = weboptions('Timeout', Inf); lidarFileName = dataFolder + "lidarPointClouds.mat"; % Check whether the folder and data file already exist or not folderExists = exist(dataFolder, 'dir'); matfilesExist = exist(lidarFileName, 'file'); % Create a new folder if it does not exist if ~folderExists mkdir(dataFolder); end % Download the lidar data if it does not exist if ~matfilesExist disp('Downloading lidarPointClouds.mat (613 MB)...'); websave(lidarFileName, baseDownloadURL + "lidarPointClouds.mat", options); end
Загрузите данные об облаке точек, сохраненные от датчика лидара Velodyne® HDL32E. Каждый скан лидара хранится как 3-D облако точек с помощью pointCloud объект. Этот объект внутренне организует данные с помощью структуры данных Kd-дерева для более быстрого поиска. Метка времени, сопоставленная с каждым сканом лидара, зарегистрирована в Time переменная timetable объект.
% Load lidar data from MAT-file data = load(lidarFileName); lidarPointClouds = data.lidarPointClouds; % Display first few rows of lidar data head(lidarPointClouds)
ans=8×1 timetable
Time PointCloud
_____________ ________________
23:46:10.5115 [1×1 pointCloud]
23:46:10.6115 [1×1 pointCloud]
23:46:10.7116 [1×1 pointCloud]
23:46:10.8117 [1×1 pointCloud]
23:46:10.9118 [1×1 pointCloud]
23:46:11.0119 [1×1 pointCloud]
23:46:11.1120 [1×1 pointCloud]
23:46:11.2120 [1×1 pointCloud]
Чтобы понять, что содержит сцена, визуализируйте записанные данные о лидаре с помощью pcplayer объект.
% Specify limits for the player xlimits = [-45 45]; % meters ylimits = [-45 45]; zlimits = [-10 20]; % Create a pcplayer to visualize streaming point clouds from lidar sensor lidarPlayer = pcplayer(xlimits, ylimits, zlimits); % Customize player axes labels xlabel(lidarPlayer.Axes, 'X (m)'); ylabel(lidarPlayer.Axes, 'Y (m)'); zlabel(lidarPlayer.Axes, 'Z (m)'); title(lidarPlayer.Axes, 'Lidar Sensor Data'); % Loop over and visualize the data for l = 1 : height(lidarPointClouds) % Extract point cloud ptCloud = lidarPointClouds.PointCloud(l); % Update lidar display view(lidarPlayer, ptCloud); end
Лидары могут использоваться, чтобы создать точные сантиметром карты, которые могут позже использоваться для локализации в транспортном средстве. Типичный подход, чтобы создать такую карту должен выровнять последовательные сканы лидара, полученные из движущегося транспортного средства, и объединить их в одно, большое облако точек. Остальная часть этого примера исследует этот подход.
Возьмите два облака точек, соответствующие соседним сканам лидара. Каждый десятый скан используется, чтобы ускорить обработку и накопить достаточно движения между сканами.
rng('default');
skipFrames = 10;
frameNum = 100;
fixed = lidarPointClouds.PointCloud(frameNum);
moving = lidarPointClouds.PointCloud(frameNum + skipFrames);Обработайте облако точек, чтобы сохранить структуры в облаке точек, которые являются отличительными. Эти шаги выполняются с помощью helperProcessPointCloud функция:
Обнаружьте и удалите наземную плоскость.
Обнаружьте и удалите автомобиль, оборудованный датчиком.
Эти шаги описаны более подробно в Наземном Обнаружении Плоскости и Препятствия Используя Лидар (Automated Driving Toolbox) пример.
fixedProcessed = helperProcessPointCloud(fixed); movingProcessed = helperProcessPointCloud(moving);
Отобразите начальные и обработанные облака точек в виде сверху. Пурпурные точки соответствуют наземной плоскости и автомобилю, оборудованному датчиком.
hFigFixed = figure; axFixed = axes('Parent', hFigFixed, 'Color', [0,0,0]); pcshowpair(fixed, fixedProcessed, 'Parent', axFixed); title(axFixed, 'Segmented Ground and Ego Vehicle'); view(axFixed, 2);
Downsample облака точек, чтобы улучшить регистрационную точность и скорость алгоритма.
gridStep = 0.5; fixedDownsampled = pcdownsample(fixedProcessed, "gridAverage", gridStep); movingDownsampled = pcdownsample(movingProcessed, "gridAverage", gridStep);
Выровняйте и объедините последовательные сканы лидара с помощью основанной на функции регистрации можно следующим образом:
Извлеките дескрипторы Быстрой гистограммы функции точки (FPFH) из каждого скана с помощью extractFPFHFeatures функция.
Идентифицируйте соответствия точки путем сравнения дескрипторов с помощью pcmatchfeatures функция.
Оцените твердое преобразование от соответствий точки с помощью estimateGeometricTransform3D функция.
Выровняйте и объедините облако точек относительно облака контрольной точки с помощью предполагаемого преобразования. Это выполняется с помощью pcalign функция.
% Extract FPFH Features for each point cloud neighbors = 40; [fixedFeature, fixedValidInds] = extractFPFHFeatures(fixedDownsampled, ... 'NumNeighbors', neighbors); [movingFeature, movingValidInds] = extractFPFHFeatures(movingDownsampled, ... 'NumNeighbors', neighbors); fixedValidPts = select(fixedDownsampled, fixedValidInds); movingValidPts = select(movingDownsampled, movingValidInds); % Identify the point correspondences method = 'Exhaustive'; threshold = 1; ratio = 0.96; indexPairs = pcmatchfeatures(movingFeature, fixedFeature, movingValidPts, ... fixedValidPts,"Method",method, "MatchThreshold",threshold, ... "RejectRatio", ratio); matchedFixedPts = select(fixedValidPts, indexPairs(:,2)); matchedMovingPts = select(movingValidPts, indexPairs(:,1)); % Estimate rigid transform of moving point cloud with respect to reference % point cloud maxDist = 2; maxNumTrails = 3000; tform = estimateGeometricTransform3D(matchedMovingPts.Location, ... matchedFixedPts.Location, "rigid", "MaxDistance", maxDist, ... "MaxNumTrials", maxNumTrails); % Transform the moving point cloud to the reference point cloud, to % visualize the alignment before and after registration movingReg = pctransform(movingProcessed, tform); % Moving and fixed point clouds are represented by magenta and green colors hFigAlign = figure; axAlign1 = subplot(1, 2, 1,'Color', [0, 0, 0], 'Parent', hFigAlign); pcshowpair(movingProcessed, fixedProcessed, 'Parent', axAlign1); title(axAlign1, 'Before Registration'); view(axAlign1, 2); axAlign2 = subplot(1, 2, 2,'Color', [0, 0, 0], 'Parent', hFigAlign); pcshowpair(movingReg, fixedProcessed, 'Parent', axAlign2); title(axAlign2, 'After Registration'); view(axAlign2, 2);
% Align and merge the point clouds alignGridStep = 1; ptCloudAccum = pcalign([fixedProcessed, movingProcessed], ... [rigid3d, tform], alignGridStep); % Visualize the accumulated point cloud hFigAccum = figure; axAccum = axes('Parent', hFigAccum, 'Color', [0,0,0]); pcshow(ptCloudAccum, 'Parent', axAccum); title(axAccum, 'Accumulated Point Cloud'); view(axAccum, 2);
Примените предварительную обработку и основанные на функции регистрационные шаги в цикле по целой последовательности записанных данных. Результатом является карта среды, пересеченной транспортным средством.
rng('default'); numFrames = height(lidarPointClouds); accumTform = rigid3d; pointCloudMap = pointCloud(zeros(0, 0, 3)); % Specify limits for the player xlimits = [-200 250]; % meters ylimits = [-150 500]; zlimits = [-100 100]; % Create a pcplayer to visualize map mapPlayer = pcplayer(xlimits, ylimits, zlimits); title(mapPlayer.Axes, 'Accumulated Map'); mapPlayer.Axes.View = [0, 90]; % Loop over the entire data to generate map for n = 1 : skipFrames : numFrames - skipFrames % Get the nth point cloud ptCloud = lidarPointClouds.PointCloud(n); % Segment ground and remove ego vehicle ptProcessed = helperProcessPointCloud(ptCloud); % Downsample the point cloud for speed of operation ptDownsampled = pcdownsample(ptProcessed, "gridAverage", gridStep); % Extract the features from point cloud [ptFeature, ptValidInds] = extractFPFHFeatures(ptDownsampled, ... 'NumNeighbors', neighbors); ptValidPts = select(ptDownsampled, ptValidInds); if n==1 moving = ptValidPts; movingFeature = ptFeature; pointCloudMap = ptValidPts; else fixed = moving; fixedFeature = movingFeature; moving = ptValidPts; movingFeature = ptFeature; % Match the features to find correspondences indexPairs = pcmatchfeatures(movingFeature, fixedFeature, moving, ... fixed, "Method",method,"MatchThreshold",threshold, ... "RejectRatio", ratio); matchedFixedPts = select(fixed, indexPairs(:,2)); matchedMovingPts = select(moving, indexPairs(:,1)); % Register moving point cloud w.r.t reference point cloud tform = estimateGeometricTransform3D(matchedMovingPts.Location, ... matchedFixedPts.Location, "rigid", "MaxDistance", maxDist, ... "MaxNumTrials", maxNumTrails); % Compute accumulated transformation to original reference frame accumTform = rigid3d(tform.T * accumTform.T); % Align and merge moving point cloud to accumulated map pointCloudMap = pcalign([pointCloudMap, moving], ... [rigid3d, accumTform], alignGridStep); end % Update map display view(mapPlayer, pointCloudMap); end
pcdownsample | extractFPFHFeatures | pcmatchfeatures | estimateGeometricTransform3D | pctransform | pcalign
Создайте карту из данных о лидаре (Automated Driving Toolbox)
Оснуйте обнаружение плоскости и препятствия Используя лидар (Automated Driving Toolbox)