selectSubmap

Выберите подкарту в рамках карты

Описание

пример

ndtMapOut = selectSubmap(ndtMapIn,roi) выбирает подкарту в рамках карты NDT ndtMapIn использование заданной необходимой области roi.

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

ndtMapOut = selectSubmap(ndtMapIn,center,sz) выбирает подкарту, заданную центром center и размер sz из подкарты.

Примеры

свернуть все

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

Необходимая область в виде вектора с 6 элементами из формы [xmin xmax ymin ymax zmin zmax].

Центр подкарты в виде вектора с 3 элементами из формы [xc yc zc].

Размер подкарты вдоль каждой оси в виде вектора с 3 элементами из формы [xsz ysz zsz].

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

свернуть все

Карта NDT, возвращенная как pcmapndt объект с обновленным SelectedSubmap свойство.

Если область выбранной подкарты находится вне пределов карты, выбранная подкарта ограничивается к пределам карты, аналогичным описанному XLimits'YLimits' , и ZLimits свойства pcmapndt объект.

Советы

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

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

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

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

Объекты

Функции

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