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 и сегменты inlier inlierSegments в дополнение к любой комбинации аргументов из предыдущих синтаксисов.

Необязательные аргументы в виде имя-значение

[___] = 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]. Порог является максимальным процентом расстояния от идеального соответствия. Функция классифицирует сегменты классифицированы как возможные совпадения, если расстояние между их векторами признаков ниже порога.

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

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

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

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

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

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

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

свернуть все

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

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

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

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

Совет

  • Удаление сегментов из карты с помощью deleteSegments, перед использованием findPose функция, может улучшить эффективность.

Алгоритмы

findPose находит абсолютное положение сегментированного облака точек с помощью алгоритма SegMatch [1] для распознавания места. Функция находит совпадения между интересующими сегментами и сегментами на карте и возвращает абсолютное положение, которое выравнивает совпадения сегментов на карте.

  • Создание карты: Обнаружение замыкания цикла - Закрытие цикла начинается с нахождения абсолютного положения путем нахождения совпадений сегмента между последним добавленным видом и функциями сегмента в выбранной подкарте, что задано SelectedSubmap свойство карты.

    Последнее добавленное представление соответствует замыканию цикла, когда findPose функция может оценить допустимое геометрическое преобразование. Если функция не может оценить это преобразование, то функция возвращает пустое значение для absPoseMap.

  • Создание карты: Правильный дрейф - Чтобы исправить дрейф, добавьте представление, которое содержит большинство инлиеров для замыкания цикла как соединения с набором представления облака точек pcviewset объект как соединение, используя addConnection функция объекта. Используйте optimizePoses функция для коррекции накопленного дрейфа.

  • Локализация - Чтобы найти абсолютное положение облака точек на карте, функция ищет сегмент, соответствующий между текущими функциями currentFeatures и подкарту, заданную как SelectedSubmap свойство sMap. Если он не может оценить допустимое геометрическое преобразование, не может быть оценено, функция возвращает пустое значение для absPoseMap выходной аргумент.

  • Визуализация - Используйте inlierFeatures и inlierSegments выводить аргументы с pcshowMatchedFeatures функция для визуализации совпадений сегментов между функциями и сегментами, включенными в карту.

Ссылки

[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.

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