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

Этот пример использует:

Открыть Live Script

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

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

Обзор

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

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

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

  • Создайте карту, используя SegMatch для обнаружения замыкания цикла

  • Локализуйте на предварительно построенной карте с помощью SegMatch

Загрузка данных

Данные, используемые в этом примере, являются частью набора данных Velodyne SLAM. Он включает примерно 6 минут данных, записанных сканером Velodyne 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. Создайте файл datastore с помощью helperReadVelodyneSLAMData функция для загрузки данных облака точек из файлов PNG и преобразования значений расстояния в координаты 3-D. Функция helper является пользовательской функцией чтения, которая предназначена для 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 состоит из четырех различных компонентов: сегментация облака точек, редукция данных, соответствие сегментов и геометрическая верификация. Для достижения наилучших результатов предварительно обработайте облако точек перед выполнением этих четырех шагов.

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

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

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

  2. Удалите землю при подготовке, чтобы сегментировать облако точек на отдельные объекты. Использование 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

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

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

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

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')

Построение карты

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

  1. Предварительно обработайте облако точек

  2. Зарегистрируйте облако точек

  3. Сегментируйте облако точек и извлекайте функции

  4. Обнаружение закрытий цикла

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

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

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 (Navigation Toolbox) .

    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] Dube, Renaud, Daniel Dugas, Elena Stumm, Juan Nieto, Roland Siegwart, and Cesar Cadena. SegMatch: Segment Based Place Recognition in 3D Облаков точек (неопр.) (недоступная ссылка). В 2017 году IEEE International Conference on Robotics and Automation (ICRA), 5266-72. Сингапур: IEEE, 2017. https://doi.org/10.1109/ICRA.2017.7989618.

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

helperReadVelodyneSLAMData считывает облака точек из файлов изображений PNG из набора данных Velodyne 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

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

Поддержка

Для просмотра документации необходимо авторизоваться на сайте