Построение карты и локализация с помощью сопоставления сегментов

В этом примере используются:

В этом примере показано, как построить карту с данными лидара и локализовать положение транспортного средства на карте с помощью SegMatch [1], алгоритма распознавания места на основе сопоставления сегментов.

Автономные системы вождения используют локализацию для определения положения транспортного средства в картографированной среде. Автономная навигация требует точной локализации, которая опирается на точную карту. Создание точной карты крупномасштабных сред затруднено, поскольку карта накапливает дрейф с течением времени, и обнаружение замыканий петель для коррекции накопленного дрейфа является сложной задачей из-за динамических препятствий. Алгоритм SegMatch надежен к динамическим препятствиям и надежен в крупномасштабных средах. Алгоритм представляет собой сегментный подход, который использует преимущества описательных форм и распознает места по совпадению сегментов.

Обзор

Как и в примере Построение карты из данных Lidar с использованием SLAM, в этом примере используются данные 3-D lidar для построения карты и корректируется для накопленного дрейфа с помощью графа SLAM. Однако этот пример не требует глобальных оценок позы от других датчиков, таких как инерциальный измерительный блок (IMU). После построения карты этот пример использует ее для локализации транспортного средства в известной среде.

В этом примере показано, как:

Используйте SegMatch, чтобы найти относительное преобразование между двумя облаками точек, которые соответствуют одному и тому же месту
Построение карты с помощью SegMatch для обнаружения замыкания контура
Локализация на предварительно созданной карте с помощью SegMatch

Загрузить данные

Данные, используемые в этом примере, являются частью набора данных Velodine SLAM. Она включает в себя приблизительно 6 минут данных, записанных сканером Velodine HDL64E-S2. Загрузите данные во временный каталог. Это может занять несколько минут.

baseDownloadURL = 'https://www.mrt.kit.edu/z/publ/download/velodyneslam/data/scenario1.zip';
dataFolder = fullfile(tempdir, 'kit_velodyneslam_data_scenario1', filesep);
options = weboptions('Timeout', Inf);

zipFileName = dataFolder + "scenario1.zip";

folderExists = exist(dataFolder, 'dir');
if ~folderExists
    % Create a folder in a temporary directory to save the downloaded zip file.
    mkdir(dataFolder)
    
    disp('Downloading scenario1.zip (153 MB) ...')
    websave(zipFileName, baseDownloadURL, options);
    
    % Unzip downloaded file.
    unzip(zipFileName, dataFolder)
end

Downloading scenario1.zip (153 MB) ...

Загрузить и выбрать данные

Загруженный набор данных хранит данные облака точек в PNG-файлах. Создание хранилища данных файла с помощью helperReadVelodyneSLAMData для загрузки данных облака точек из файлов PNG и преобразования значений расстояния в 3-D координаты. Вспомогательная функция представляет собой пользовательскую функцию чтения, которая предназначена для Velodyne SLAM Dataset. Выберите подмножество данных и разделите данные, которые будут использоваться для построения карты и локализации.

% Get the full file path to the PNG files in the scenario1 folder.
pointCloudFilePattern = fullfile(dataFolder, 'scenario1', 'scan*.png');

% Create a file datastore to read files in the right order.
fileDS = fileDatastore(pointCloudFilePattern, 'ReadFcn', ...
    @helperReadVelodyneSLAMData);

% Read the point clouds.
ptCloudArr = readall(fileDS);

% Select a subset of point cloud scans, and split the data to use for
% map building and for localization.
ptCloudMap = vertcat(ptCloudArr{1:5:1550});
ptCloudLoc = vertcat(ptCloudArr{2:5:1550});

% Visualize the first point cloud.
figure
pcshow(ptCloudMap(1))
title('Point Cloud Data')

Обзор SegMatch

Алгоритм SegMatch состоит из четырех различных компонентов: сегментация облака точек, извлечение элементов, сопоставление сегментов и геометрическая проверка. Для получения наилучших результатов перед выполнением этих четырех шагов предварительно обработайте облако точек.

Предварительная обработка облака точек

Чтобы выбрать наиболее релевантные данные облака точек, выполните следующие шаги предварительной обработки.

Выберите цилиндрический район, центрированный вокруг транспортного средства, чтобы извлечь интересующее локальное облако точек. Сначала задайте цилиндрическую окрестность на основе расстояния точек от начала координат в направлениях x и y. Затем выберите интересующую область с помощью select.
Удалите грунт при подготовке к разделению облака точек на отдельные объекты. Использовать segmentGroundSMRF для сегментации земли.

% Select a point cloud from the map for preprocessing.
ptCloud = ptCloudMap(25);

% Set the cylinder radius and ego radius.
cylinderRadius = 40;
egoRadius = 1;

% Compute the distance between each point and the origin.
dists = hypot(ptCloud.Location(:,:,1), ptCloud.Location(:,:,2));

% Select the points inside the cylinder radius and outside the ego radius.
cylinderIdx = dists <= cylinderRadius & dists >= egoRadius;
cylinderPtCloud = select(ptCloud, cylinderIdx, 'OutputSize', 'full');

% Remove the ground.
[~, ptCloudNoGround] = segmentGroundSMRF(cylinderPtCloud, 'ElevationThreshold', 0.05);

% Visualize the point cloud before and after preprocessing.
figure
pcshowpair(ptCloud, ptCloudNoGround)
title('Point Cloud Before and After Preprocessing')

Сегментация и извлечение элементов

Далее следует сегментировать облако точек и извлекать элементы из каждого сегмента.

Сегментирование облака точек с помощью segmentLidarData и визуализировать сегменты. В этом примере каждый сегмент должен иметь не менее 150 точек. Это приводит к созданию кластеров сегментов, которые представляют различные объекты и имеют достаточно точек для характеристики области на карте.

Различные наборы данных требуют различных параметров для сегментации. Требование меньшего количества точек для сегментов может привести к ложному положительному замыканию контура, а ограничение сегментов более крупными кластерами может устранить сегменты, важные для распознавания места. Необходимо также настроить пороговые значения расстояния и угла, чтобы каждый сегмент соответствовал одному объекту. Малый порог расстояния может привести к множеству сегментов, которые соответствуют одному и тому же объекту, а большой порог расстояния и малый порог угла могут привести к сегментам, которые объединяют множество объектов.

minNumPoints = 150;
distThreshold = 1;
angleThreshold = 180;
[labels, numClusters] = segmentLidarData(ptCloudNoGround, distThreshold, ...
    angleThreshold, 'NumClusterPoints', minNumPoints);

% Remove points that contain a label value of 0 for visualization.
idxValidPoints = find(labels);
labelColorIndex = labels(idxValidPoints);
segmentedPtCloud = select(ptCloudNoGround, idxValidPoints);

figure
pcshow(segmentedPtCloud.Location, labelColorIndex)
title('Point Cloud Segments')

Извлеките элементы из каждого сегмента с помощью extractEigenFeatures функция. Элементы на основе собственных значений являются геометрическими элементами. Каждый характерный вектор включает линейность, планарность, рассеяние, всепроникающую способность, анизотропию, собственную энтропию и изменение кривизны.

[features, segments] = extractEigenFeatures(ptCloud, labels);
disp(features)

  31×1 eigenFeature array with properties:

    Feature
    Centroid

disp(segments)

  31×1 pointCloud array with properties:

    Location
    Count
    XLimits
    YLimits
    ZLimits
    Color
    Normal
    Intensity

Согласование сегментов и геометрическая проверка

Найдите совпадающие сегменты и преобразование между сегментами для двух сканирований облака точек, которые соответствуют замыканию контура.

Предварительная обработка и извлечение элементов сегмента из двух облаков точек. helperPreProcessPointCloud функция включает шаги предварительной обработки в разделе «Облако точек предварительной обработки», чтобы упростить предварительную обработку облака точек в течение всего рабочего процесса.

ptCloud1 = ptCloudMap(27);
ptCloud2 = ptCloudMap(309);

ptCloud1 = helperPreProcessPointCloud(ptCloud1, egoRadius, cylinderRadius);
ptCloud2 = helperPreProcessPointCloud(ptCloud2, egoRadius, cylinderRadius);

labels1 = segmentLidarData(ptCloud1, distThreshold, ...
    angleThreshold, 'NumClusterPoints', minNumPoints);
labels2 = segmentLidarData(ptCloud2, distThreshold, ...
    angleThreshold, 'NumClusterPoints', minNumPoints);

[features1, segments1] = extractEigenFeatures(ptCloud1, labels1);
[features2, segments2] = extractEigenFeatures(ptCloud2, labels2);

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

featureMatrix1 = vertcat(features1.Feature);
featureMatrix2 = vertcat(features2.Feature);
indexPairs = pcmatchfeatures(featureMatrix1, featureMatrix2);

Выполните геометрическую проверку, определив вложенные элементы и найдя 3-D жесткое преобразование между совпадениями сегментов с помощью estimateGeometricTransform3D функция. В зависимости от количества входов облака точек можно классифицировать как замыкание контура.

centroids1 = vertcat(features1(indexPairs(:,1)).Centroid);
centroids2 = vertcat(features2(indexPairs(:,2)).Centroid);
[tform, inlierPairs] = estimateGeometricTransform3D(centroids1, centroids2, 'rigid');

Визуализация соответствий сегментов с помощью pcshowMatchedFeatures функция.

inlierIdx1 = indexPairs(inlierPairs,1);
inlierIdx2 = indexPairs(inlierPairs,2);
figure
pcshowMatchedFeatures(segments1(inlierIdx1), segments2(inlierIdx2), ...
    features1(inlierIdx1), features2(inlierIdx2))
title('Segment Matches')

Выровнять сегменты с преобразованием из шага геометрической проверки с помощью pccat и pctransform.

ptCloudSegments1 = pccat(segments1);
ptCloudSegments2 = pccat(segments2);
tformedPtCloudSegments1 = pctransform(ptCloudSegments1, tform);

Визуализация выровненных сегментов с помощью pcshowpair.

figure
pcshowpair(tformedPtCloudSegments1, ptCloudSegments2)
title('Aligned Segments')

Карта построения

Процедура построения карты состоит из следующих шагов:

Предварительная обработка облака точек
Регистрация облака точек
Сегментация облака точек и извлечение элементов
Обнаружение замыканий петель

Предварительная обработка облака точек

Предварительная обработка предыдущего и текущего облака точек с помощью helperPreProcessPointCloud. Понизить значения точечных облаков с помощью pcdownsample повышение скорости и точности регистрации. Чтобы настроить входной процент пониженной выборки, найдите наименьшее значение, которое поддерживает требуемую точность регистрации при повороте транспортного средства.

currentViewId = 2;

prevPtCloud = helperPreProcessPointCloud(ptCloudMap(currentViewId - 1), ...
    egoRadius, cylinderRadius);
ptCloud = helperPreProcessPointCloud(ptCloudMap(currentViewId), ...
    egoRadius, cylinderRadius);

downsamplePercent = 0.5;

prevPtCloudFiltered = pcdownsample(prevPtCloud, 'random', downsamplePercent);
ptCloudFiltered = pcdownsample(ptCloud, 'random', downsamplePercent);

Регистрация облака точек

Зарегистрируйте текущее облако точек с предыдущим облаком точек, чтобы найти относительное преобразование.

gridStep = 3;
relPose = pcregisterndt(ptCloudFiltered, prevPtCloudFiltered, gridStep);

Использовать pcviewset объект для отслеживания абсолютных поз и связей между зарегистрированными облаками точек. Создать пустой pcviewset.

vSet = pcviewset;

Инициализируйте представление первого облака точек в жестком преобразовании идентификации и добавьте его в набор видов с помощью addView.

initAbsPose = rigid3d;
vSet = addView(vSet, currentViewId - 1, initAbsPose);

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

absPose = rigid3d(relPose.T * initAbsPose.T);
vSet = addView(vSet, currentViewId, absPose);

Соедините два вида с помощью addConnection.

vSet = addConnection(vSet, currentViewId - 1, currentViewId, relPose);

Трансформируйте текущее облако точек, чтобы выровнять его по глобальной карте.

ptCloud = pctransform(ptCloud, absPose);

Облако точек сегмента и элементы извлечения

Сегментировать предыдущее и текущее облака точек с помощью segmentLidarData.

labels1 = segmentLidarData(prevPtCloud, distThreshold, angleThreshold, ...
    'NumClusterPoints', minNumPoints);
labels2 = segmentLidarData(ptCloud, distThreshold, angleThreshold, ...
    'NumClusterPoints', minNumPoints);

Извлечение элементов из предыдущих и текущих сегментов облака точек с помощью extractEigenFeatures.

[prevFeatures, prevSegments] = extractEigenFeatures(prevPtCloud, labels1);
[features, segments] = extractEigenFeatures(ptCloud, labels2);

Отслеживание сегментов и элементов с помощью pcmapsegmatch объект. Создать пустой pcmapsegmatch.

sMap = pcmapsegmatch;

Добавление видов, элементов и сегментов для предыдущего и текущего облаков точек в pcmapsegmatch использование addView.

sMap = addView(sMap, currentViewId - 1, prevFeatures, prevSegments);
sMap = addView(sMap, currentViewId, features, segments);

Обнаружение замыканий петель

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

Обнаружение замыканий шлейфа с помощью findPose.

[absPoseMap, loopClosureViewId] = findPose(sMap, absPose);
isLoopClosure = ~isempty(absPoseMap);

Если findPose обнаруживает замыкание контура, находит преобразование между текущим видом и видом замыкания контура и добавляет его в pcviewset объект.

Используйте абсолютную позу текущего вида без накопленного дрейфа, absPoseMapи абсолютную позу вида замыкания контура, absPoseLoop, для вычисления относительной позы между позициями замыкания петли без дрейфа.

if isLoopClosure
    absPoseLoop = poses(vSet, loopClosureViewId).AbsolutePose;
    relPoseLoopToCurrent = rigid3d(absPoseMap.T * invert(absPoseLoop).T);

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

    vSet = addConnection(vSet, loopClosureViewId, currentViewId, ...
        relPoseLoopToCurrent);

Исправьте накопленный дрейф с помощью оптимизации графика позы. Рассмотрите возможность поиска нескольких соединений замыкания контура перед оптимизацией поз, так как оптимизация графика позы и обновление pcmapsegmatch оба объекта являются вычислительно интенсивными.

Сохраните позы перед оптимизацией. Позы необходимы для обновления сегментов и расположений центроидов в pcmapsegmatch объект.

    prevPoses = vSet.Views.AbsolutePose;

Создание графика позы из набора видов с помощью createPoseGraphи оптимизировать график позы с помощью optimizePoseGraph(Панель инструментов навигации).

    G = createPoseGraph(vSet);     
    optimG = optimizePoseGraph(G, 'g2o-levenberg-marquardt');
    vSet = updateView(vSet, optimG.Nodes);

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

    optimizedPoses = vSet.Views.AbsolutePose;
    
    relPoseOpt = rigid3d.empty;
    for k = 1:numel(prevPoses)
        relPoseOpt(k) = rigid3d(invert(prevPoses(k)).T * ...
            optimizedPoses(k).T);
    end
    
    sMap = updateMap(sMap, relPoseOpt);
end

Чтобы построить карту и исправить накопленный дрейф, примените эти шаги к остальным сканированиям облака точек.

% Set the random seed for example reproducibility.
rng(0)

% Update display every 5 scans.
figure
updateRate = 5;

% Initialize variables for registration.
prevPtCloud = ptCloudFiltered;
prevPose = rigid3d;

% Keep track of the loop closures to optimize the poses once enough loop
% closures are detected.
totalLoopClosures = 0;

for i = 3:numel(ptCloudMap)
    ptCloud = ptCloudMap(i);
    
    % Preprocess and register the point cloud.
    ptCloud = helperPreProcessPointCloud(ptCloud, egoRadius, cylinderRadius);
    ptCloudFiltered = pcdownsample(ptCloud, 'random', downsamplePercent);
    relPose = pcregisterndt(ptCloudFiltered, prevPtCloud, gridStep, ...
        'InitialTransform', relPose);   
    ptCloud = pctransform(ptCloud, absPose);
    
    % Store the current point cloud to register the next point cloud.
    prevPtCloud = ptCloudFiltered;    
    
    % Compute the absolute pose of the current point cloud.
    absPose = rigid3d(relPose.T * absPose.T);
    
    % If the vehicle has moved at least 2 meters since last time, continue
    % with segmentation, feature extraction, and loop closure detection.
    if norm(absPose.Translation - prevPose.Translation) >= 2
        
        % Segment the point cloud and extract features.
        labels = segmentLidarData(ptCloud, distThreshold, angleThreshold, ...
            'NumClusterPoints', minNumPoints);
        [features, segments] = extractEigenFeatures(ptCloud, labels);
        
        % Keep track of the current view id.
        currentViewId = currentViewId + 1;
        
        % Add the view to the point cloud view set and map representation.
        vSet = addView(vSet, currentViewId, absPose);
        vSet = addConnection(vSet, currentViewId-1, currentViewId, ...
            rigid3d(absPose.T * invert(prevPose).T));
        sMap = addView(sMap, currentViewId, features, segments);
        
        % Update the view set display.
        if mod(currentViewId, updateRate) == 0
            plot(vSet)
            drawnow
        end

        % Check if there is a loop closure.
        [absPoseMap, loopClosureViewId] = findPose(sMap, absPose, 'MatchThreshold', 1, ...
            'MinNumInliers', 5, 'NumSelectedClusters', 4, 'NumExcludedViews', 150);
        isLoopClosure = ~isempty(absPoseMap);
        
        if isLoopClosure
            totalLoopClosures = totalLoopClosures + 1;
            
            % Find the relative pose between the loop closure poses.
            absPoseLoop = poses(vSet, loopClosureViewId).AbsolutePose;
            relPoseLoopToCurrent = rigid3d(absPoseMap.T * invert(absPoseLoop).T);
            vSet = addConnection(vSet, loopClosureViewId, currentViewId, ...
                relPoseLoopToCurrent);
            
            % Optimize the graph of poses and update the map every time 3
            % loop closures are detected.
            if mod(totalLoopClosures, 3) == 0
                prevPoses = vSet.Views.AbsolutePose;
                
                % Correct for accumulated drift.
                G = createPoseGraph(vSet);
                optimG = optimizePoseGraph(G, 'g2o-levenberg-marquardt');
                vSet = updateView(vSet, optimG.Nodes);
                
                % Update the map.
                optimizedPoses = vSet.Views.AbsolutePose;
                relPoseOpt = rigid3d.empty;
                for k = 1:numel(prevPoses)
                    relPoseOpt(k) = rigid3d(invert(prevPoses(k)).T * ...
                        optimizedPoses(k).T);
                end
                sMap = updateMap(sMap, relPoseOpt);
                
                % Update the absolute pose after pose graph optimization.
                absPose = optimizedPoses(end);
            end
        end
        prevPose = absPose;
    end
end

% Visualize the map of segments from the top view.
figure
show(sMap)
view(2)
title('Map of Segments')

Локализация транспортного средства на известной карте

Шаги предварительной обработки для локализации с использованием SegMatch являются теми же шагами предварительной обработки, которые используются для построения карты. Так как алгоритм опирается на согласованную сегментацию, используйте те же параметры сегментации для наилучших результатов.

ptCloud = ptCloudLoc(1);

% Preprocess the point cloud.
ptCloud = helperPreProcessPointCloud(ptCloud, egoRadius, cylinderRadius);

% Segment the point cloud and extract features.  
labels = segmentLidarData(ptCloud, distThreshold, angleThreshold, ...
    'NumClusterPoints', minNumPoints);
features = extractEigenFeatures(ptCloud, labels);

Поскольку оценка местоположения транспортного средства отсутствует, необходимо использовать объем карты для начальной локализации транспортного средства. Выберите область карты, которую необходимо локализовать в первый раз с помощью selectSubmap.

sMap = selectSubmap(sMap, [sMap.XLimits, sMap.YLimits, sMap.ZLimits]);

Используйте findPose объектная функция pcmapsegmatch для локализации транспортного средства на предварительно созданной карте.

absPoseMap = findPose(sMap, features, 'MatchThreshold', 1, 'MinNumInliers', 5);

Визуализация карты и использование showShape для визуализации транспортного средства на карте в виде кубоида.

mapSegments = pccat(sMap.Segments);
hAxLoc = pcshow(mapSegments.Location, 'p');
title('Localization on a Prebuilt Map')
view(2)

poseTranslation = absPoseMap.Translation;
eul = rotm2eul(absPoseMap.Rotation');
theta = rad2deg(eul);
pos = [poseTranslation, 5, 9, 3.5, theta(2), theta(3), theta(1)];
showShape('cuboid', pos, 'Color', 'green', 'Parent', hAxLoc, 'Opacity', 0.8, 'LineWidth', 0.5)

Чтобы повысить скорость локализации для остальных сканирований, выберите подкарту с помощью selectSubmap.

submapSize = [65, 65, 200];
sMap = selectSubmap(sMap, poseTranslation, submapSize);

Продолжите локализацию транспортного средства, используя остальную часть сканирования облака точек. Использовать isInsideSubmap и selectSubmap для обновления подкарты. Если не хватает сегментов для локализации транспортного средства с помощью сопоставления сегментов, используйте регистрацию для оценки позы.

% Visualize the map.
figure('Visible', 'on')
hAx = pcshow(mapSegments.Location, 'p');
title('Localization on a Prebuilt Map')

% Set parameter to update submap.
submapThreshold = 30;

% Initialize the poses and previous point cloud for registration.
prevPtCloud = ptCloud;
relPose = rigid3d;
prevAbsPose = rigid3d;

% Segment each point cloud and localize by finding segment matches.
for n = 2:numel(ptCloudLoc)
    ptCloud = ptCloudLoc(n);
    
    % Preprocess the point cloud.
    ptCloud = helperPreProcessPointCloud(ptCloud, egoRadius, cylinderRadius);
    
    % Segment the point cloud and extract features.
    labels = segmentLidarData(ptCloud, distThreshold, angleThreshold, ...
        'NumClusterPoints', minNumPoints);
    features = extractEigenFeatures(ptCloud, labels);
    
    % Localize the point cloud.
    absPoseMap = findPose(sMap, features, 'MatchThreshold', 1, 'MinNumInliers', 5);

    % Do registration when the position cannot be estimated with segment
    % matching.
    if isempty(absPoseMap)
        relPose = pcregisterndt(ptCloud, prevPtCloud, gridStep, ...
            'InitialTransform', relPose);
        absPoseMap = rigid3d(relPose.T * prevAbsPose.T);
    end

    % Display position estimate in the map.
    poseTranslation = absPoseMap.Translation;
    eul = rotm2eul(absPoseMap.Rotation');
    theta = rad2deg(eul);
    pos = [poseTranslation, 5, 9, 3.5, theta(2), theta(3), theta(1)];
    showShape('cuboid', pos, 'Color', 'green', 'Parent', hAx, 'Opacity', 0.8, 'LineWidth', 0.5)

    % Determine if selected submap needs to be updated.
    [isInside, distToEdge] = isInsideSubmap(sMap, poseTranslation);
    needSelectSubmap = ~isInside ...                      % Current pose is outside submap.
        || any(distToEdge(1:2) < submapThreshold);        % Current pose is close to submap edge.

    % Select a new submap.
    if needSelectSubmap
        sMap = selectSubmap(sMap, poseTranslation, submapSize);
    end

    prevAbsPose = absPoseMap;
    prevPtCloud = ptCloud;
end

Ссылки

[1] Дюбе, Рено, Даниэль Дугас, Елена Штумм, Хуан Ньето, Роланд Зигварт и Сесар Кадена. «SegMatch: распознавание места на основе сегментов в облаках точек 3D». В 2017 году IEEE Международная конференция по робототехнике и автоматизации (ICRA), 5266-72. Сингапур: IEEE, 2017. https://doi.org/10.1109/ICRA.2017.7989618.

Вспомогательные функции

helperReadVelodyneSLAMData считывает облака точек из файлов изображений PNG из набора данных Velodine SLAM.

helperPreProcessPointCloud выбирает цилиндрическую окрестность и удаляет грунт из облака точек.

function ptCloud = helperPreProcessPointCloud(ptCloud, egoRadius, cylinderRadius)

% Compute the distance between each point and the origin.
dists = hypot(ptCloud.Location(:,:,1), ptCloud.Location(:,:,2));

% Select the points inside the cylinder radius and outside the ego radius.
cylinderIdx = dists <= cylinderRadius & dists >= egoRadius;
ptCloud = select(ptCloud, cylinderIdx, 'OutputSize', 'full');

% Remove ground.
[~, ptCloud] = segmentGroundSMRF(ptCloud, 'ElevationThreshold', 0.05);

end

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

Построение карты и локализация с помощью сопоставления сегментов

Обзор

Загрузить данные

Загрузить и выбрать данные

Обзор SegMatch

Предварительная обработка облака точек

Сегментация и извлечение элементов

Согласование сегментов и геометрическая проверка

Карта построения

Предварительная обработка облака точек

Регистрация облака точек

Облако точек сегмента и элементы извлечения

Обнаружение замыканий петель

Локализация транспортного средства на известной карте

Ссылки

Вспомогательные функции

Документация по панели инструментов Lidar

Поддержка