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