Создать график 3-D позы
A poseGraph3D объект сохраняет информацию для 3-D представления графика позы. График позы содержит узлы, связанные рёбрами. Каждая оценка узла связана с графиком граничными ограничениями, которые определяют относительную позу между узлами и неопределенность этого измерения.
Для итеративного построения графа позы используйте addRelativePose функция для добавления относительных оценок позы и их соединения с существующим узлом с указанными граничными ограничениями. Узлы позы должны быть указаны относительно узла позы. Задайте неопределенность измерения с помощью информационной матрицы.
Добавление ребра между двумя несравненными узлами создает замыкание цикла на графике. Также поддерживается множество кромок или множественных кромок между парами узлов, что включает замыкания контуров. Чтобы добавить дополнительные ограничения кромок или замыкания контуров, укажите идентификаторы узлов с помощью addRelativePose функция. При оптимизации графика позы optimizePoseGraph функция находит решение, удовлетворяющее всем этим ограничениям кромок.
Чтобы добавить узлы точек ориентира, используйте addPointLandmark функция. Эта функция определяет узлы как точки xyz без оценок ориентации. Ориентиры должны указываться относительно узла позы.
Для получения 2-D графиков позы см. раздел poseGraph.
Пример построения и оптимизации графика 3-D позы на основе реальных данных датчиков см. в разделе Landmark SLAM Using WitchingTag Markers.
poseGraph = poseGraph3D создает объект графика 3-D позы. Добавить позы с помощью addRelativePose для итеративного построения графа позы.
poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) задает верхнюю границу для числа рёбер и узлов, разрешенных в графе позы при создании кода. Этот предел требуется только при создании кода.
addPointLandmark | Добавление узла точки ориентира для представления графика |
addRelativePose | Добавить относительную позу к графу позы |
copy | Создание копии графика позы |
edgeNodePairs | Пары рёберных узлов в графе позы |
edgeConstraints | Ограничения ребер на графике позы |
edgeResidualErrors | Вычислить остаточные ошибки ребра графика позы |
findEdgeID | Найти идентификатор края |
nodeEstimates | Положения узлов в графе позы |
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]
LandmarkNodeIDs: [1x0 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] Карлоне, Лука, Роберто Трон, Костас Даниилидис и Фрэнк Деллаерт. «Методы инициализации для 3D SLAM: исследование по оценке ротации и ее использованию в оптимизации графика позы». 2015 IEEE Международная конференция по робототехнике и автоматизации (ICRA). 2015, стр 4597–4604.