Выбор подкарты в карте
Загрузите карту преобразования нормальных распределений (NDT) из файла MAT.
data = load('ndtMapParkingLot.mat');
ndtMap = data.ndtMapParkingLot;Загрузка результатов сканирования облака точек и оценок позы из второго файла MAT.
data = load('parkingLotData.mat');
ptCloudScans = data.parkingLotData.ptCloudScans;
initPoseEsts = data.parkingLotData.initPoseEsts;Отображение карты неразрушающего контроля.
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 функция, но она также может уменьшить частоту обновления подкарты.