Создайте 2D график положения
poseGraph объект сохраняет информацию для 2D представления графика положения. График положения содержит узлы, соединенные ребрами. Каждая оценка узла соединяется с графиком ограничениями ребра, которые задают относительное положение между узлами и неопределенностью на том измерении.
Чтобы создать график положения итеративно, используйте addRelativePose функция, чтобы добавить родственника излагает оценки и соединить их с существующим узлом с заданными ограничениями ребра. Позируйте узлы должны быть заданы относительно узла положения. Задайте неопределенность в измерении с помощью информационной матрицы.
Добавление ребра между двумя непоследовательными узлами создает закрытие цикла в графике. Несколько ребер или мультиребер между парами узла также поддерживаются, который включает закрытия цикла. Чтобы добавить дополнительные ограничения ребра или закрытия цикла, задайте идентификаторы узла с помощью addRelativePose функция. При оптимизации графика положения, optimizePoseGraph функция находит решение удовлетворить всем этим ограничениям ребра.
Чтобы добавить узлы поворотного момента, используйте addPointLandmark функция. Эта функция задает узлы, когда xy - указывает без оценок ориентации. Ориентиры должны быть заданы относительно узла положения.
lidarSLAM объект выполняет основанную на лидаре одновременную локализацию и картографию, которая базируется вокруг оптимизации 2D графика положения.
Для 3-D графиков положения смотрите poseGraph3D возразите или Ориентир SLAM Используя пример Маркеров AprilTag.
poseGraph = poseGraph создает 2D объект графика положения. Добавьте использование положений addRelativePose создать график положения итеративно.
poseGraph = poseGraph('MaxNumEdges',maxEdges,'MaxNumNodes',maxNodes) задает верхнюю границу на количестве ребер и узлов, позволенных в графике положения при генерации кода. Этот синтаксис только требуется при генерации кода.
addPointLandmark | Добавьте узел поворотного момента в график положения |
addRelativePose | Добавьте относительное положение в график положения |
copy | Создайте копию графика положения |
edgeNodePairs | Пары узла ребра в графике положения |
edgeConstraints | Ограничения ребра в графике положения |
edgeResidualErrors | Вычислите остаточные ошибки ребра графика положения |
findEdgeID | Найдите ID ребра ребра |
nodeEstimates | Положения узлов в графике положения |
removeEdges | Удалите ребра закрытия цикла из графика |
show | Постройте график положения |
Оптимизируйте график положения на основе ограничений ребра и узлов. График положения, используемый в этом примере, от научно-исследовательской лаборатории Intel Набор данных и был сгенерирован от собирающейся одометрии колеса и лазерной информации о датчике средства поиска области значений во внутренней лаборатории.
Загрузите набор данных Intel, который содержит 2D график положения. Смотрите 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] Гризетти, G., Р. Каммерл, К. Стэкнисс и В. Бергард. "Пример на Основанном на графике SLAM". IEEE Интеллектуальный Системный Журнал Транспортировки. Издание 2, № 4, 2010, стр 31–43. doi:10.1109/mits.2010.939925.