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 элементами формы [<<reservedrangesplaceholder5> <reservedrangesplaceholder4> <reservedrangesplaceholder3> <reservedrangesplaceholder2> <reserved angesplaceholder1> <reservedrangesplaceholder0> ].

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

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

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

свернуть все

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

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

Совет

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

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

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

.

См. также

Объекты

Функции

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