В этом примере показано, как обрабатывать данные 3-D лидара от датчика, установленного на транспортном средстве, для постепенного построения карты. Такая карта подходит для автоматического управления рабочими процессами, такими как локализация и навигация. Эти карты можно использовать для локализации транспортного средства в пределах нескольких сантиметров.
Существуют различные способы регистрации точечных облаков. Типичный подход заключается в использовании полного облака точек для регистрации. В примере построения карты на основе данных Lidar (Automated Driving Toolbox) этот подход используется для построения карты. В этом примере для построения карты используются отличительные элементы, извлеченные из облака точек.
В этом примере вы узнаете, как:
Загрузка и визуализация записанных данных вождения.
Построение карты с помощью сканирования lidar.
Данные, используемые в этом примере, представляют приблизительно 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
Загрузите данные облака точек, сохраненные с датчика Velodine ® HDL32E lidar. Каждое лидарное сканирование сохраняется как 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 функция:
Обнаружение и удаление плоскости заземления.
Обнаружение и удаление эго-транспортного средства.
Эти шаги более подробно описаны в примере «Наземная плоскость и обнаружение препятствий с помощью Lidar (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);
Чтобы повысить точность регистрации и скорость алгоритма, выполните пониженную выборку облаков точек.
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
Построение карты на основе данных Lidar (автоматизированная панель инструментов вождения)
Обнаружение нулевой плоскости и препятствий с помощью Lidar (автоматизированная панель инструментов вождения)