findPose

Локализуйте облако точек в карте с помощью алгоритма преобразования нормальных распределений (NDT)

Свернуть все на странице

Синтаксис

currPose = findPose(ndtMap,ptCloud,initPose)
currPose = findPose(___,Name,Value)

Описание

пример

currPose = findPose(ndtMap,ptCloud,initPose) локализует положение облака точек ptCloud в пределах карты NDT ndtMap использование алгоритма NDT. Функция ограничивает пространство поиска подкартой, заданной SelectedSubmap свойство ndtMap объект.

currPose = findPose(___,Name,Value) задает опции с использованием одного или нескольких аргументов в дополнение к входным параметрам в предыдущем синтаксисе. Для примера 'MaxIterations', 30 устанавливает максимальное количество итераций, прежде чем функция остановит алгоритм NDT.

Примеры

свернуть все

Открыть Live Script

Загрузите карту преобразования нормальных распределений (NDT) из файла MAT.

data = load('ndtMapParkingLot.mat');
ndtMap = data.ndtMapParkingLot;

Загрузите сканы облака точек и поместите оценки из второго файла MAT.

data = load('parkingLotData.mat');
ptCloudScans = data.parkingLotData.ptCloudScans;
initPoseEsts = data.parkingLotData.initPoseEsts;

Отобразите карту NDT.

show(ndtMap)

Измените угол обзора на вид сверху.

view(2)

Выберите подкарту с центром вокруг первой оценки.

center = initPoseEsts(1).Translation;
sz = [70 50 20];
ndtMap = selectSubmap(ndtMap,center,sz);

Установите радиус для визуализации текущего местоположения и порог расстояния, чтобы обновить подкарту.

radius = 0.5;
distThresh = 15;

Закольцовывайте облака точек, локализуйте их на карте и обновляйте выбранную подкарту по мере необходимости.

numScans   = numel(ptCloudScans);
  
for n = 1:numScans
    ptCloud = ptCloudScans(n);
    initPose = initPoseEsts(n);

    poseTranslation = initPose.Translation;
    [isInside,distToEdge] = isInsideSubmap(ndtMap,poseTranslation);
    submapNeedsUpdate = ~isInside ...       % Current pose is outside submap
        || any(distToEdge(1:2) < distThresh);   % Current pose is close to submap edge

if submapNeedsUpdate
    ndtMap = selectSubmap(ndtMap,poseTranslation,sz);
end

% Localize the point cloud scan in the map.
currPose = findPose(ndtMap,ptCloud,initPose);

% Display the position of the estimate as a circle.
pos = [currPose.Translation(1:2) radius]; 
showShape('circle',pos,'Color','r');

% Pause to view the change.
pause(0.05)
end

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

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

свернуть все

Карта NDT, заданная как pcmapndt объект.

Облако точек в системе координат датчика, заданное как pointCloud объект.

Начальная оценка положения датчика на карте, заданная как rigid3d объект.

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

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

Пример: 'MaxIterations',30 останавливает алгоритм NDT после 30 итераций.

Ожидаемый процент выбросов относительно нормального распределения, заданный как скаляр в области значений [0, 1). Алгоритм NDT предполагает, что точка генерируется смесью нормального распределения для инлиеров и равномерного распределения для выбросов. Большее значение 'OutlierRatio' уменьшает влияние выбросов.

Типы данных: single | double

Максимальное количество итераций перед остановкой алгоритма NDT, заданное как неотрицательное целое число.

Типы данных: single | double

Допуск между последовательными итерациями NDT, заданный как 2-элементный вектор с неотрицательными значениями. Вектор [Tdiff Rdiff] представляет допуск абсолютного различия в перемещении и вращении, соответственно, оцененный в последовательных итерациях NDT. Tdiff измеряет евклидово расстояние между двумя векторами преобразования. Rdiff измеряет угловое различие в степенях. Алгоритм останавливается, когда различие между предполагаемыми твердыми преобразованиями в самых последних последовательных итерациях падает ниже заданных значений допуска.

Типы данных: single | double

Отображение информации о прогрессе, заданной как логическое 0 (false) или 1 (true). Задайте 'Verbose' на true отображение прогресса информации.

Типы данных: logical

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

свернуть все

Положение датчика на карте, возвращаемое как rigid3d объект. Функция ограничивает пространство поиска подкартой, заданной SelectedSubmap свойство ndtMap.

Совет

  • Чтобы улучшить точность и эффективность локализации, рассмотрите понижение дискретизации облака точек с помощью pcdownsample перед использованием этой функции.

Ссылки

Бибер, П. и У. Страссер. Преобразование нормальных распределений: новый подход к лазерному Сопоставлению сканов. В Трудах 2003 IEEE/RSJ Международная конференция по интеллектуальным роботам и системам (IROS 2003) (Кат. No.03CH37453) том 3, 2743-48. Лас-Вегас, Невада, США: IEEE, 2003. https://doi.org/10.1109/IROS.2003.1249285.

[1] Магнуссон, Мартин. Трехмерное преобразование нормальных распределений: эффективное представление для регистрации, анализа поверхности и обнаружения цикла. Докторская диссертация, университет Эребру, 2009. http://urn.kb.se/resolve?urn=urn: nbn: se: oru: diva-8458 urn: nbn: se: oru: diva-8458.

Расширенные возможности

Генерация кода C/C + +
Сгенерируйте код C и C++ с помощью Coder™ MATLAB ®

.

См. также

Объекты

Функции

Введенный в R2021a

Документация по Computer Vision Toolbox

Поддержка

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