Основанные на функциях Создания карты из данных Лидара

Открыть Live Script

Этот пример демонстрирует, как обработать 3-D данные лидара с датчика, установленного на транспортном средстве, чтобы постепенно создавать карту. Такая карта подходит для автоматизированных рабочих процессов вождения, таких как локализация и навигация. Эти карты могут использоваться для локализации транспортного средства в пределах нескольких сантиметров.

Обзор

Существуют различные способы регистрации облаков точек. Типичным подходом является использование полного облака точек для регистрации. Создайте карту из примера Lidar Data (Automated Driving Toolbox), использует этот подход для создания карты. Этот пример использует отличительные функции, извлеченные из облака точек для создания карты.

В этом примере вы научитесь:

Загрузка и визуализация зарегистрированных данных вождения.
Создайте карту с помощью сканов лидара.

Загрузка записанных приводных данных

Данные, используемые в этом примере, представляют приблизительно 100 секунд данных лидара, GPS и БИНС. Данные сохраняются в отдельных 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

Загрузите данные облака точек, сохраненные с датчика лидара HDL32E Velodyne ®. Каждый скан лидара хранится как 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 функция:

Обнаружить и удалить наземную плоскость.
Обнаружение и удаление автомобиля , оборудованного датчиком.

Эти шаги более подробно описаны в примере Ground Plane and Brestacle Detection Using 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);

Регистрация на основе функций

Выровнять и объединить последующие сканы лидара с помощью регистрации на основе признаков можно следующим образом:

Извлечение дескрипторов функции Fast Point Feature Histogram (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

Функции

Объекты

pcplayer | pointCloud

Внешние веб-сайты

Udacity Self-Driving Car Data Subset (репозиторий MathWorks GitHub)

Документация