Найдите абсолютное положение в карте, которая выравнивает соответствия сегмента
absPoseMap = findPose(sMap,refPose)SelectedSubmap свойство sMap.
[ возвращает идентификатор представления для представления, которое содержит большую часть inliers. Используйте 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 inlier в дополнение к любой комбинации аргументов от предыдущих синтаксисов.
[___] = findPose(___, задает опции с помощью одних или нескольких аргументов name-value в дополнение к входным параметрам в предыдущих синтаксисах. Например, Name,Value)'MaxThreshold',1.5 устанавливает соответствующий порог к 1.5 процент.
Загрузите карту сегментов и функций из файла MAT. Данные об облаке точек в карте были собраны с помощью блока Simulation 3D Lidar (UAV Toolbox).
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] для распознавания места. Функция находит соответствия между сегментами интереса и сегментами в карте, и возвращает абсолютное положение, которое выравнивает соответствия сегмента в карте.
Создание карты: Обнаружение Закрытия Цикла — закрытие Цикла запускается с нахождения абсолютного положения путем нахождения соответствий сегмента между последним добавленным представлением и функциями сегмента в выбранной подкарте, которая задана SelectedSubmap свойство карты.
Последнее добавленное представление соответствует закрытию цикла когда findPose функция может оценить допустимое геометрическое преобразование. Если функция не может оценить это преобразование, то функция возвращает пустое значение для absPoseMap.
Создание карты: Правильный Дрейф — Чтобы откорректировать для дрейфа, добавляет представление, которое содержит большую часть inliers закрытия цикла for, когда связь с представлением облака точек установила pcviewset возразите как связь с помощью addConnection объектная функция. Используйте optimizePoses функционируйте, чтобы откорректировать для накопленного дрейфа.
Локализация — Чтобы найти абсолютное положение облака точек в карте, функция ищет соответствия сегмента между текущими функциями currentFeatures и подкарта задана SelectedSubmap свойство sMap. Если это не может оценить, что допустимое геометрическое преобразование не может быть оценено, функция возвращает пустое значение для absPoseMap выходной аргумент.
Визуализация — использует inlierFeatures и inlierSegments выходные аргументы с pcshowMatchedFeatures функция, чтобы визуализировать соответствия сегмента между функциями и сегментами, включенными в карту.
[1] Дубе, Renaud, Дэниел Дугас, Елена Стамм, Хуан Ньето, Роланд Сигварт и Сесар Кадена. “SegMatch: Распознавание Сегмент Бэзед-Плэйс в 3D Облаках точек”. На 2 017 Международных конференциях IEEE по вопросам Робототехники и Автоматизации (ICRA), 5266–72. Сингапур, Сингапур: IEEE, 2017. https://doi.org/10.1109/ICRA.2017.7989618.