Создать график 2-D позы
A poseGraph объект сохраняет информацию для 2-D представления графика позы. График позы содержит узлы, связанные рёбрами. Каждая оценка узла связана с графиком граничными ограничениями, которые определяют относительную позу между узлами и неопределенность этого измерения.
Для итеративного построения графа позы используйте addRelativePose функция для добавления относительных оценок позы и их соединения с существующим узлом с указанными граничными ограничениями. Узлы позы должны быть указаны относительно узла позы. Задайте неопределенность измерения с помощью информационной матрицы.
Добавление ребра между двумя несравненными узлами создает замыкание цикла на графике. Также поддерживается множество кромок или множественных кромок между парами узлов, что включает замыкания контуров. Чтобы добавить дополнительные ограничения кромок или замыкания контуров, укажите идентификаторы узлов с помощью addRelativePose функция. При оптимизации графика позы optimizePoseGraph функция находит решение, удовлетворяющее всем этим ограничениям кромок.
Чтобы добавить узлы точек ориентира, используйте addPointLandmark функция. Эта функция определяет узлы как точки xy без оценок ориентации. Ориентиры должны указываться относительно узла позы.
lidarSLAM объект выполняет основанную на лидаре одновременную локализацию и отображение, основанное на оптимизации графа 2-D позы.
Для получения 3-D графиков позы см. poseGraph3D объект или пример SLAM ориентира с использованием маркеров тегов.
poseGraph = poseGraph создает объект графика 2-D позы. Добавить позы с помощью addRelativePose для итеративного построения графа позы.
poseGraph = poseGraph('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) задает верхнюю границу для числа рёбер и узлов, разрешенных в графе позы при создании кода. Этот синтаксис требуется только при создании кода.
addPointLandmark | Добавление узла точки ориентира для представления графика |
addRelativePose | Добавить относительную позу к графу позы |
copy | Создание копии графика позы |
edgeNodePairs | Пары рёберных узлов в графе позы |
edgeConstraints | Ограничения ребер на графике позы |
edgeResidualErrors | Вычислить остаточные ошибки ребра графика позы |
findEdgeID | Найти идентификатор края |
nodeEstimates | Положения узлов в графе позы |
removeEdges | Удаление кромок замыкания контура из графика |
show | График позы графика |
Оптимизируйте график позы на основе узлов и ограничений кромок. График позы, использованный в этом примере, взят из набора данных Intel Research Lab Dataset и был создан на основе сбора информации об одометрии колес и датчике лазерного дальномера в лаборатории внутри помещений.
Загрузите набор данных Intel, содержащий график 2-D позы. Осмотрите poseGraph объект для просмотра количества узлов и замыканий цикла.
load intel-2d-posegraph.mat pg disp(pg)
poseGraph with properties:
NumNodes: 1228
NumEdges: 1483
NumLoopClosureEdges: 256
LoopClosureEdgeIDs: [1x256 double]
LandmarkNodeIDs: [1x0 double]
Постройте график позы с предлагаемыми идентификаторами. Красные линии указывают замыкания петель, определенные в наборе данных.
show(pg,'IDs','off'); title('Original Pose Graph')
Оптимизируйте график позы. Узлы настраиваются на основе ограничений кромок и замыканий контуров. Постройте график оптимизированной позы, чтобы увидеть настройку узлов с замыканиями цикла.
updatedPG = optimizePoseGraph(pg); figure show(updatedPG,'IDs','off'); title('Updated Pose Graph')
[1] Гризетти, Г., Р. Куммерле, К. Стахнисс и У. Бургард. «Учебное пособие по SLAM на основе графиков». Журнал интеллектуальных транспортных систем IEEE. Том 2, № 4, 2010, стр. 31-43. doi: 10.1109/mits.2010.939925.