Создайте 3-D график положения
poseGraph3D информация об объектно-ориентированной памяти для 3-D представления графика положения. График положения содержит nodes соединенный edges, с ограничениями ребра, которые задают относительное положение между узлами и неопределенностью на том измерении. optimizePoseGraph функция изменяет узлы, чтобы составлять неопределенность и улучшить полный график.
Для 2D графиков положения смотрите poseGraph.
Чтобы создать график положения итеративно, используйте addRelativePose добавить положения и соединить их с существующим графиком. Укажите, что неопределенность сопоставила использование информационной матрицы. Укажите, что закрытия цикла добавляют дополнительные ограничения ребра между существующими узлами.
poseGraph = poseGraph3D создает 3-D объект диаграмм положения. Добавьте положения с помощью addRelativePose создать график положения итеративно.
poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) задает верхнюю границу на количестве ребер и узлов, позволенных в графике положения при генерации кода. Этот предел только требуется при генерации кода.
addRelativePose | Добавьте относительное положение, чтобы изложить график |
edges | Ребра в графике положения |
edgeConstraints | Ограничения ребра в графике положения |
findEdgeID | Найдите ID ребра ребра |
nodes | Положения узлов в графике положения |
optimizePoseGraph | Оптимизируйте узлы в графике положения |
removeEdges | Удалите ребра закрытия цикла из графика |
show | Постройте график положения |
Оптимизируйте график положения на основе ограничений ребра и узлов. График положения, используемый в этом примере, взят из Набора данных MIT и был сгенерирован с помощью информации, извлеченной из гаража.
Загрузите график положения от набора данных MIT. Смотрите poseGraph3D возразите, чтобы просмотреть количество закрытий цикла и узлов.
load parking-garage-posegraph.mat pg disp(pg);
poseGraph3D with properties:
NumNodes: 1661
NumEdges: 6275
NumLoopClosureEdges: 4615
LoopClosureEdgeIDs: [1x4615 double]
Постройте график положения с идентификаторами прочь. Красные линии указывают на закрытия цикла, идентифицированные в наборе данных.
title('Original Pose Graph') show(pg,'IDs','off'); view(-30,45)
Оптимизируйте график положения. Узлы настроены на основе ограничений ребра и закрытий цикла. Постройте оптимизированный график положения, чтобы видеть корректировку узлов с закрытиями цикла.
updatedPG = optimizePoseGraph(pg); figure title('Updated Pose Graph') show(updatedPG,'IDs','off'); view(-30,45)
[1] Carlone, Лука, Роберто Трон, Костас Даниилидис и Франк Деллэерт. "Методы инициализации для 3D SLAM: Обзор Оценки Вращения и ее Использования в Оптимизации Графика Положения". 2 015 Международных конференций IEEE по вопросам Робототехники и Автоматизации (ICRA). 2015, стр 4597–4604.