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