Карта локализации на основе нормальных распределений преобразовывают (NDT)
pcmapndt объект создает карту нормальных распределений преобразовывают (NDT) из предварительно созданной карты облака точек среды. Карта NDT является сжатым, эффективным памятью представлением, подходящим для локализации. Объект преобразует карту облака точек в набор вокселей (3-D поля), каждый представленный 3-D нормальным распределением. Используйте selectSubmap возразите функции, чтобы выбрать подкарту в рамках карты от крупной оценки положения. Используйте findPose возразите функции, чтобы локализовать положение датчика на основе собранной карты.
ndtMap = pcmapndt(ptCloudMap,voxelSize)ptCloudMap.
selectSubmap | Выберите подкарту в рамках карты |
isInsideSubmap | Проверяйте, является ли положение запроса внутренней выбранной подкартой |
findPose | Локализуйте облако точек в рамках карты с помощью алгоритма нормальных распределений преобразовывают (NDT) |
show | Визуализируйте карту нормальных распределений преобразовывают (NDT) |
Загрузите набор представления облака точек, который был сохранен от pcviewset объект.
data = load('vSetPointClouds.mat');
vSet = data.vSet;Извлеките облака точек и абсолютные положения, чтобы создать карту.
ptClouds = vSet.Views.PointCloud; tforms = vSet.Views.AbsolutePose;
Создайте карту облака точек путем сопоставления сканов облака точек с помощью абсолютных положений.
ptCloudMap = pcalign(ptClouds,tforms);
Создайте и визуализируйте карту NDT из карты облака точек.
voxelSize = 1;
ndtMap = pcmapndt(ptCloudMap,voxelSize);
figure
show(ndtMap)
view(2) % Change viewing angle to top-view
Загрузите карту нормальных распределений преобразовывают (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
Бибер, P. и В. Стрэссер. “Преобразование Нормальных распределений: Новый Подход к Лазерному Сопоставлению сканов”. В Продолжениях 2003 Международных конференции IEEE/RSJ по вопросам Интеллектуальных Роботов и Систем (IROS 2003) (CAT. № 03CH37453) Издание 3, 2743-48. Лас-Вегас, Невада, США: IEEE, 2003. https://doi.org/10.1109/IROS.2003.1249285.
[1] Магнуссон, Мартин. "3D Преобразование Нормальных распределений: Эффективное Представление forRegistration, Поверхностный Анализ и Обнаружение Цикла". Диссертация, Örebro universitet, 2009. http://urn.kb.se / твердость? urn=urn:nbn:se:oru:diva-8458 urn:nbn:se:oru:diva-8458.