findPose

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

Описание

пример

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

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

Примеры

свернуть все

Загрузите карту нормальных распределений преобразовывают (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
Для просмотра документации необходимо авторизоваться на сайте