Exponenta Banner

Создайте карту и локализуйте Используя соответствие сегмента

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

Скрипт Open Live Script

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

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

Обзор

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

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

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

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

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

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

Данные, используемые в этом примере, являются частью Набора данных SLAM Velodyne. Это включает приблизительно 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";

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

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)

elseif folderExists && numel(dir(pointCloudFilePattern)) < numExpectedFiles
    % Redownload the data if it got reduced in the temporary directory.
    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 координаты. Функция помощника является пользовательской функцией чтения, которая спроектирована для Набора данных SLAM Velodyne. Выберите подмножество данных и разделите данные, чтобы использовать для создания карты и для локализации.

% 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 (Lidar Toolbox), чтобы сегментировать землю.

% 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 (Lidar Toolbox) функция. Основанными на собственном значении функциями являются геометрические функции. Каждый характеристический вектор включает линейность, планарность, рассеивание, omnivariance, анизотропию, eigenentropy, и изменение в искривлении.

[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 функция включает шаги предварительной обработки в раздел 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 (Lidar Toolbox) функция.

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

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

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

Визуализируйте соответствия сегмента при помощи pcshowMatchedFeatures (Lidar Toolbox) функция.

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. Downsample облака точек с помощью 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 (Lidar Toolbox).

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

Отследите сегменты и функции с помощью pcmapsegmatch Объект (Lidar Toolbox). Создайте пустой pcmapsegmatch (Lidar Toolbox).

sMap = pcmapsegmatch;

Добавьте представления, функции и сегменты для облаков предыдущей и текущей точки к pcmapsegmatch (Lidar Toolbox) с помощью addView (Lidar Toolbox).

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

Обнаружьте закрытия цикла

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

Обнаружьте закрытия цикла с помощью findPose (Lidar Toolbox).

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

Если findPose (Lidar Toolbox) обнаруживает закрытие цикла, найдите, что преобразование между текущим представлением и закрытием цикла просматривает и добавляет его в 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 Объект (Lidar Toolbox) оба в вычислительном отношении интенсивен.

Сохраните положения перед оптимизацией. Положения необходимы, чтобы обновить сегменты и центроидные местоположения в pcmapsegmatch Объект (Lidar Toolbox).

    prevPoses = vSet.Views.AbsolutePose;

Создайте график положения из набора представления с помощью createPoseGraph, и оптимизируйте график положения с помощью optimizePoseGraph (Navigation Toolbox) .

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

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

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

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

Используйте findPose Функция объекта (Lidar Toolbox) pcmapsegmatch (Lidar Toolbox), чтобы локализовать транспортное средство на предварительно созданной карте.

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;
quat = quaternion(absPoseMap.Rotation', 'rotmat', 'point');
theta = eulerd(quat, 'ZYX', 'point');
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 (Lidar Toolbox).

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

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

% 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;
    quat = quaternion(absPoseMap.Rotation', 'rotmat', 'point');
    theta = eulerd(quat, 'ZYX', 'point');
    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] Р. Дубе, Д. Дугас, E. Штумм, Х. Ньето, Р. Сигварт и К. Кэдена. "SegMatch: основанное на сегменте распознавание места в 3D облаках точек". Международная конференция IEEE по вопросам робототехники и автоматизации (ICRA), 2017.

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

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

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

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

Поддержка