findPose

Найдите абсолютное положение в карте, которая выравнивает соответствия сегмента

Описание

Сопоставьте создание

absPoseMap = findPose(sMap,refPose) находит абсолютное положение последнего добавленного представления, которое выравнивает соответствия сегмента обнаруженного закрытия цикла. Функция ищет соответствия сегмента между последним добавленным представлением и функциями сегмента в подкарте, заданной SelectedSubmap свойство sMap.

[absPoseMap,matchViewId] = findPose(sMap,refPose) возвращает идентификатор представления для представления, которое содержит большую часть inliers. Используйте matchViewId добавить закрытие цикла как связь в pcviewset, использование addConnection объектная функция. Правильный для накопленного использования дрейфа optimizePoses.

Локализация

absPoseMap = findPose(sMap,currentFeatures) находит абсолютное положение, которое выравнивает сегменты, которые соответствуют текущим функциям currentFeatures к сегментам в подкарте, заданной SelectedSubmap свойство sMap.

absPoseMap = findPose(sMap,currentFeatures,currentSegments) задает сегменты currentSegments это соответствует текущим функциям currentFeatures.

Визуализация

[___,inlierFeatures,inlierSegments] = findPose(___) возвращается inlier показывает inlierFeatures и сегменты inlierSegments inlier в дополнение к любой комбинации аргументов от предыдущих синтаксисов.

Дополнительные аргументы значения имени

[___] = findPose(___,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

Figure contains an axes. The axes contains an object of type scatter.

% Visualize the last segment matches.
figure; 
pcshowMatchedFeatures(inlierSegments(:,1),inlierSegments(:,2),inlierFeatures(:,1),inlierFeatures(:,2))

Figure contains an axes. The axes contains 5 objects of type scatter, line.

Входные параметры

свернуть все

Карта сегментов и функций в виде pcmapsegmatch объект.

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

Текущие функции в виде M - вектор элемента из eigenFeature объекты.

Текущие сегменты в виде M - вектор элемента из pointCloud объекты.

Аргументы в виде пар имя-значение

Задайте дополнительные разделенные запятой пары Name,Value аргументы. Name имя аргумента и Value соответствующее значение. Name должен появиться в кавычках. Вы можете задать несколько аргументов в виде пар имен и значений в любом порядке, например: Name1, Value1, ..., NameN, ValueN.

Пример: 'MatchThreshold',1.5 устанавливает соответствующий порог к 1,5 процентам.

Соответствие с порогом в виде скаляра в области значений (0, 100]. Порог является максимальным процентом расстояния от идеальной пары. Функция классифицирует сегменты, классифицируются как возможные соответствия, если расстояние между их характеристическими векторами ниже, чем порог.

Минимальное количество inliers в виде скаляра, больше, чем или равный 3. Уменьшение этого значения может привести к ложным положительным сторонам. Если количество обнаруженного inliers меньше 'MinNumInliers', функция возвращает пустой выходной параметр для absPoseMap .

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

Функция использует значение по умолчанию 10 для создания карты и 0 для локализации.

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

Количество самых близких функций, выбранных как кандидаты соответствия функции в виде положительного целого числа. Для каждой функции в последнем добавленном представлении, или в текущих функциях currentFeatures, функция выбирает самый близкий 'NumNearestNeighbor' функции как функция кандидата соответствуют. Задайте большее значение для этого аргумента для карт с многочисленными подобными функциями.

Количество кластеров функции, чтобы проверять на соответствия в виде положительного целого числа. Функциональный кандидат кластеров показывает на основе их центроидных местоположений. Если вы задаете refPose, затем findPose функция выбирает кластеры, самые близкие к центроидам последнего добавленного представления currentFeatures. Уменьшите это значение, чтобы улучшать производительность за счет увеличения вероятности ложных отрицательных сторон.

Выходные аргументы

свернуть все

Абсолютное положение в карте, возвращенной как rigid3d объект. Этот объект задает абсолютное положение, которое выравнивает соответствия сегмента.

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

Функции Inlier, возвращенные как N-by-2 матрица eigenFeature объекты. Первый столбец соответствует inliers в карте, и второй столбец соответствует inliers в последнем добавленном представлении или текущем входе функций.

Inlier сегменты, возвращенные как N-by-2 матрица pointCloud объекты. Первый столбец соответствует inliers в карте, и второй столбец соответствует inliers в последнем добавленном представлении или текущем входе сегментов.

Советы

  • Удаление сегментов от использования карты 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.

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