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

Скрипт Open Live Script

Этот пример демонстрирует, как обработать 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

Функции

Объекты

pcplayer | pointCloud

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

Подмножество Данных Беспилотного автомобиля Udacity (репозиторий MathWorks GitHub)

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