Найти абсолютную позу на карте, которая выравнивает совпадения сегментов
absPoseMap = findPose(sMap,refPose)SelectedSubmap имущество sMap.
[ возвращает идентификатор представления, содержащего наибольшее количество вложенных элементов. Использовать absPoseMap,matchViewId] = findPose(sMap,refPose)matchViewId добавление замыкания контура в качестве соединения в pcviewset, с использованием addConnection объектная функция. Исправить для накопленного дрейфа с помощью optimizePoses.
absPoseMap = findPose(sMap,currentFeatures)currentFeatures к сегментам в подкарте, указанной SelectedSubmap имущество sMap.
absPoseMap = findPose(sMap,currentFeatures,currentSegments)currentSegments которые соответствуют текущим характеристикам currentFeatures.
[___, возвращает элементы inlier inlierFeatures,inlierSegments] = findPose(___)inlierFeatures и более внутренние сегменты inlierSegments в дополнение к любой комбинации аргументов из предыдущих синтаксисов.
[___] = findPose(___, указывает параметры, использующие один или несколько аргументов «имя-значение» в дополнение к входным аргументам в предыдущих синтаксисах. Например, Name,Value)'MaxThreshold',1.5 устанавливает порог соответствия равным 1.5 процентов.
Загрузите карту сегментов и элементов из файла MAT. Данные облака точек на карте собраны с помощью блока Simulation 3D Lidar (Панель инструментов БПЛА).
data = load('segmatchMapFullParkingLot.mat');
sMap = data.segmatchMapFullParkingLot;Загрузка сканирования облака точек из файла MAT.
data = load('fullParkingLotData.mat');
ptCloudScans = data.fullParkingLotData;Отображение карты сегментов.
ax = show(sMap);
Измените угол обзора на вид сверху.
view(2) pause(0.2)
Задайте радиус для выбора цилиндрической окрестности.
outerCylinderRadius = 20; innerCylinderRadius = 3;
Задайте пороговые параметры сегментации.
distThreshold = 0.5; angleThreshold = 180;
Установка пороговых параметров размера и подкарты для выбранной подкарты
sz = [65 30 20]; submapThreshold = 10;
Задайте параметр радиуса для визуализации.
radius = 0.5;
Сегментируйте каждое облако точек и локализуйте, найдя совпадения сегментов.
for n = 1:numel(ptCloudScans) ptCloud = ptCloudScans(n); % Segment and remove the ground plane. groundPtsIdx = segmentGroundFromLidarData(ptCloud,'ElevationAngleDelta',11); ptCloud = select(ptCloud,~groundPtsIdx,'OutputSize','full'); % Select the cylindrical neighborhood. dists = sqrt(ptCloud.Location(:,:,1).^2 + ptCloud.Location(:,:,2).^2); cylinderIdx = dists <= outerCylinderRadius & dists > innerCylinderRadius; ptCloud = select(ptCloud,cylinderIdx,'OutputSize','full'); % Segment the point cloud. labels = segmentLidarData(ptCloud,distThreshold,angleThreshold,'NumClusterPoints',[50 5000]); % Extract features from the point cloud. [features,segments] = extractEigenFeatures(ptCloud,labels); % Localize by finding the absolute pose in the map that aligns the segment matches. [absPoseMap,~,inlierFeatures,inlierSegments] = findPose(sMap,features,segments); if isempty(absPoseMap) continue; end % Display the position estimate in the map. poseTranslation = absPoseMap.Translation; pos = [poseTranslation(1:2) radius]; showShape('circle',pos,'Color','r','Parent',ax); pause(0.2) % Determine if the 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 || n == 1; % 1st time localizing using whole map % Select a new submap. if needSelectSubmap sMap = selectSubmap(sMap,poseTranslation,sz); end end
% Visualize the last segment matches.
figure;
pcshowMatchedFeatures(inlierSegments(:,1),inlierSegments(:,2),inlierFeatures(:,1),inlierFeatures(:,2))
Удаление сегментов с карты с помощью deleteSegments, перед использованием findPose функция, может улучшить производительность.
findPose находит абсолютную позу сегментированного облака точек с помощью алгоритма SegMatch [1] для распознавания места. Функция находит совпадения между интересующими сегментами и сегментами на карте и возвращает абсолютную позу, которая выравнивает совпадения сегментов на карте.
Map Building: Loop Closure Detection - Замыкание контура начинается с нахождения абсолютной позы путем нахождения совпадений сегментов между последним добавленным видом и элементами сегмента в выбранной подкарте, которая определяется SelectedSubmap свойства карты.
Последнее добавленное представление соответствует замыканию цикла, когда findPose может оценить допустимое геометрическое преобразование. Если функция не может оценить это преобразование, то функция возвращает пустое значение для absPoseMap.
Построение карты: Правильный дрейф - чтобы исправить дрейф, добавьте вид, который содержит больше всего внутренних элементов для замыкания контура в качестве соединения с набором видов облака точек pcviewset объект как соединение с использованием addConnection объектная функция. Используйте optimizePoses функция для коррекции накопленного дрейфа.
Локализация - чтобы найти абсолютную позу облака точек на карте, функция ищет совпадения сегментов между текущими элементами currentFeatures и подкарту, указанную SelectedSubmap имущество sMap. Если невозможно оценить допустимое геометрическое преобразование, функция возвращает пустое значение для absPoseMap выходной аргумент.
Визуализация - используйте inlierFeatures и inlierSegments выходные аргументы с pcshowMatchedFeatures функция для визуализации совпадений сегментов между элементами и сегментами, включенными в карту.
[1] Дюбе, Рено, Даниэль Дугас, Елена Штумм, Хуан Ньето, Роланд Зигварт и Сесар Кадена. «SegMatch: распознавание места на основе сегментов в облаках точек 3D». В 2017 году IEEE Международная конференция по робототехнике и автоматизации (ICRA), 5266-72. Сингапур, Сингапур: IEEE, 2017. https://doi.org/10.1109/ICRA.2017.7989618.