Создайте 3-D график положения
A poseGraph3D
объект хранит информацию для представления графика положения 3-D. График положения содержит узлы, соединенные ребрами. Каждая оценка узла связана с графиком ограничениями ребра, которые определяют относительное положение между узлами и неопределенность в этом измерении.
Чтобы создать график положения итерационно, используйте addRelativePose
функция, чтобы добавить относительные оценки положения и соединить их с существующим узлом с заданными ограничениями ребра. Узлы положения должны быть заданы относительно узла положения. Задайте неопределенность измерения с помощью информационной матрицы.
Добавление ребра между двумя несовпадающими узлами создает замыкание цикла в графике. Несколько ребер или мультиребра между парами узлов также поддерживаются, что включает в себя закрытие цикла. Чтобы добавить дополнительные ограничения по ребрам или замыкания циклов, задайте идентификаторы узла, используя addRelativePose
функция. При оптимизации графика положения, optimizePoseGraph
функция находит решение, чтобы удовлетворить всем этим ограничениям ребра.
Чтобы добавить узлы точек ориентира, используйте addPointLandmark
функция. Эта функция задает узлы как xyz -точки без оценок ориентации. Ориентиры должны быть заданы относительно узла положения.
Для 2-D графиков положения смотрите poseGraph
.
Для примера, который строит и оптимизирует график положения 3-D на основе данных о датчике реального мира, см. Landmark SLAM Использование AprilTag Markers.
poseGraph = poseGraph3D
создает объект графиков положения 3-D. Добавьте положения используя addRelativePose
чтобы создать график положения итеративно.
poseGraph = poseGraph3D('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes)
задает верхнюю границу количества ребер и узлов, разрешенных в графике положения при генерации кода. Этот предел требуется только при генерации кода.
addPointLandmark | Добавьте узел точки ориентира в график положения |
addRelativePose | Добавьте относительное положение в график положения |
copy | Создайте копию графика положения |
edgeNodePairs | Пары узлов ребер в графике положения |
edgeConstraints | Ограничения по ребрам в графике положения |
edgeResidualErrors | Вычислите остаточные ошибки ребра графика положения |
findEdgeID | Найдите идентификатор ребра ребра |
nodeEstimates | Положения узлов в графике положения |
removeEdges | Удалите ребра замыкания цикла из графика |
show | Постройте график положения |
[1] Carlone, Luca, Roberto Tron, Kostas Daniilidis и Frank Dellaert. «Initialization Методов for 3D SLAM: a Survey on Rotation Estimation and Use in Pose Graph Optimization». 2015 IEEE International Conference on Robotics and Automation (ICRA). 2015, стр 4597–4604.