Выберите подкарту в рамках карты
Загрузите карту нормальных распределений преобразовывают (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
Используйте размер подкарты, достаточно большой, чтобы включать неопределенность в оценках положения и область значений датчика, используемого с findPose. Большая подкарта может увеличить время вычисления во время каждого вызова findPose функция, но это может также уменьшить частоту обновлений подкарты.