Exponenta Banner

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.

Примеры

свернуть все

Скрипт Open 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 принимает, что точка сгенерирована смесью нормального распределения для inliers и равномерного распределения для выбросов. Большее значение 'OutlierRatio' уменьшает влияние выбросов.

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

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

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

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

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

Отобразите информацию о прогрессе в виде логического 0 ложь) или 1 TRUE). Установите 'Verbose' к true отобразить информацию о прогрессе.

Типы данных: логический

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

свернуть все

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

Советы

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

Ссылки

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

[1] Магнуссон, Мартин. "3D Преобразование Нормальных распределений: Эффективное Представление forRegistration, Поверхностный Анализ и Обнаружение Цикла". Диссертация, Örebro universitet, 2009. http://urn.kb.se / твердость? urn=urn:nbn:se:oru:diva-8458 urn:nbn:se:oru:diva-8458.

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

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

Смотрите также

Объекты

Функции

Введенный в R2021a

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

Поддержка