Создайте 3-D график положения
poseGraph3D
объект сохраняет информацию для 3-D представления графика положения. График положения содержит узлы, соединенные ребрами. Каждая оценка узла соединяется с графиком ограничениями ребра, которые задают относительное положение между узлами и неопределенностью на том измерении.
Чтобы создать график положения итеративно, используйте addRelativePose
функция, чтобы добавить родственника излагает оценки и соединить их с существующим узлом с заданными ограничениями ребра. Позируйте узлы должны быть заданы относительно узла положения. Задайте неопределенность в измерении с помощью информационной матрицы.
Добавление ребра между двумя непоследовательными узлами создает закрытие цикла в графике. Несколько ребер или мультиребер между парами узла также поддерживаются, который включает закрытия цикла. Чтобы добавить дополнительные ограничения ребра или закрытия цикла, задайте идентификаторы узла с помощью addRelativePose
функция. При оптимизации графика положения, optimizePoseGraph
функция находит решение удовлетворить всем этим ограничениям ребра.
Чтобы добавить узлы поворотного момента, используйте addPointLandmark
функция. Эта функция задает узлы, когда xyz - указывает без оценок ориентации. Ориентиры должны быть заданы относительно узла положения.
Для 2D графиков положения смотрите poseGraph
.
Для примера, который создает и оптимизирует 3-D график положения из реальных данных о датчике, смотрите Ориентир SLAM Используя Маркеры AprilTag.
poseGraph = poseGraph3D
создает 3-D объект графика положения. Добавьте использование положений addRelativePose
создать график положения итеративно.
poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes)
задает верхнюю границу на количестве ребер и узлов, позволенных в графике положения при генерации кода. Этот предел только требуется при генерации кода.
addPointLandmark | Добавьте узел поворотного момента в график положения |
addRelativePose | Добавьте относительное положение в график положения |
copy | Создайте копию графика положения |
edgeNodePairs | Пары узла ребра в графике положения |
edgeConstraints | Ограничения ребра в графике положения |
edgeResidualErrors | Вычислите остаточные ошибки ребра графика положения |
findEdgeID | Найдите ID ребра ребра |
nodeEstimates | Положения узлов в графике положения |
removeEdges | Удалите ребра закрытия цикла из графика |
show | Постройте график положения |
[1] Carlone, Лука, Роберто Трон, Костас Даниилидис и Франк Деллэерт. "Методы инициализации для 3D SLAM: Обзор Оценки Вращения и ее Использования в Оптимизации Графика положения". 2 015 Международных конференций IEEE по вопросам Робототехники и Автоматизации (ICRA). 2015, стр 4597–4604.