Exponenta Banner
exponenta event banner

findPose

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

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

Синтаксис

currPose = findPose (ndtMap, ptCloud, initPose)
currPose = findPose (___, имя, значение)

Описание

пример

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

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

Примеры

свернуть все

Открыть сценарий в реальном времени

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

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

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

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

Отображение карты неразрушающего контроля.

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.

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

свернуть все

Карта неразрушающего контроля, заданная как pcmapndt объект.

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

Первоначальная оценка позы датчика на карте, указанная как rigid3d объект.

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

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

Пример: 'MaxIterations',30 останавливает алгоритм неразрушающего контроля после 30 итераций.

Ожидаемый процент отклонений относительно нормального распределения, заданного как скаляр в диапазоне [0, 1). Алгоритм неразрушающего контроля предполагает, что точка генерируется смесью нормального распределения для входов и равномерного распределения для выходов. Большее значение '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 перед использованием этой функции.

Ссылки

Бибер, П. и В. Штрассер. «Преобразование нормальных распределений: новый подход к сопоставлению лазерного сканирования». Международная конференция IEEE/RSJ по интеллектуальным роботам и системам (IROS 2003) (Cat. 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++ с помощью MATLAB ® Coder™

.

См. также

Объекты

Функции

Представлен в R2021a

Документация по инструментарию компьютерного зрения

Поддержка