Exponenta Banner

optimizePoseGraph

Оптимизируйте узлы в графике положения

Свернуть все на странице

Синтаксис

updatedGraph = optimizePoseGraph(poseGraph)
updatedGraph = optimizePoseGraph(poseGraph,solver)
[updatedGraph,solutionInfo] = optimizePoseGraph(___)
[___] = optimizePoseGraph(___,Name,Value)

Описание

пример

updatedGraph = optimizePoseGraph(poseGraph) корректирует положения на основе их ребра ограничений, определенных в указанном графе, чтобы улучшить общий график. Вы оптимизируете 2-D или 3-D график положения. Возвращенный график положения имеет ту же топологию с обновленными узлами.

Эта оптимизация графика положения предполагает, что все ограничения ребра и замыкания цикла действительны. Чтобы рассмотреть возможность обрезки ребер на основе плохих замыканий цикла, смотрите trimLoopClosures функция.

updatedGraph = optimizePoseGraph(poseGraph,solver) задает тип решателя для оптимизации графика положения.

[updatedGraph,solutionInfo] = optimizePoseGraph(___) возвращает дополнительную статистику о процессе оптимизации в solutionInfo использование любого из предыдущих синтаксисов.

[___] = optimizePoseGraph(___,Name,Value) задает дополнительные опции, используя один или несколько Name,Value пар. Для примера, 'MaxIterations',1000 увеличивает максимальное количество итераций до 1000.

Примеры

свернуть все

Открыть Live Script

Оптимизируйте график положения на основе узлов и ограничений по ребрам. График положения, используемый в этом примере, взят из набора данных 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)

Figure contains an axes. The axes contains 3 objects of type line.

Оптимизируйте график положения. Узлы корректируются на основе ограничений ребра и цикла замыканий. Постройте график оптимизированного графика положения, чтобы увидеть настройку узлов с закрытиями контура.

updatedPG = optimizePoseGraph(pg);
figure
title('Updated Pose Graph')
show(updatedPG,'IDs','off');
view(-30,45)

Figure contains an axes. The axes contains 3 objects of type line.

Входные параметры

свернуть все

2-D или 3-D график положения, заданный как poseGraph, poseGraph3D, digraph объект.

Как использовать digraph объект, сгенерируйте график положения используя createPoseGraph из imageviewset или pcviewset объект. У вас должны быть Computer Vision Toolbox™ и solver должно быть установлено в "builtin-trust-region". The 'LoopClosuresToIgnore' и 'FirstNodePose' пары "имя-значение" игнорируются, если задано.

Область ребер digraph объект описывается affine3d (Image Processing Toolbox) или rigid3d (Image Processing Toolbox) объекты.

Решатель графика положения, заданный как "builtin-trust-region" или "g2o-levenberg-marquardt". Чтобы настроить любой решатель, используйте аргументы пары "имя-значение" для этого решателя.

Аргументы в виде пар имя-значение

Задайте необязательные разделенные разделенными запятой парами Name,Value аргументы. Name - имя аргумента и Value - соответствующее значение. Name должны находиться внутри кавычек. Можно задать несколько аргументов в виде пар имен и значений в любом порядке Name1,Value1,...,NameN,ValueN.

Пример: 'MaxTime', 300

Примечание

В зависимости от solver вход, функция поддерживает различные пары "имя-значение".

Если на solver для входов задано значение "builtin-trust-region":

свернуть все

Максимально допустимое время, заданное как разделенная разделенными запятой парами, состоящая из 'MaxTime' и положительный числовой скаляр в секундах. Оптимизатор выходит после того, как превысит это время.

Нижняя граница нормы градиента, заданная как разделенная разделенными запятой парами, состоящая из 'GradientTolerance' и скаляром. Норма градиента вычисляется на основе функции затрат оптимизации. Если норма падает ниже этого значения, оптимизатор выходит.

Нижняя граница изменения функции затрат, заданная как разделенная разделенными запятой парами, состоящая из 'FunctionTolerance' и скаляром. Если изменение функции стоимости падает ниже этого значения между шагами оптимизации, оптимизатор выходит.

Нижняя граница размера шага, заданная как разделенная разделенными запятой парами, состоящая из 'StepTolerance' и скаляром. Если норма шага оптимизации падает ниже этого значения, оптимизатор выходит.

Радиус начальной доверительной области, заданный как скаляр.

Отображение информации о промежуточной итерации в MATLAB® командная строка, заданная как разделенная разделенными запятой парами, состоящая из 'VerboseOutput' и любой из них 'off' или 'on'.

Идентификаторы ребер замыкания цикла в poseGraph, заданная как разделенная разделенными запятой парами, состоящая из 'LoopClosuresToIgnore' и вектор. Чтобы получить идентификаторы ребер из графика положения, используйте findEdgeID.

Положение первого узла в poseGraph, заданная как разделенная разделенными запятой парами, состоящая из 'FirstNodePose' и вектор положения.

Для poseGraph (2-D), положение является [x y theta] вектор, который задает относительное xy -положение и угол ориентации, theta.

Для poseGraph3D, положение является [x y z qw qx qy qz] вектор, который определяет относительное xyz -положение и кватернионную ориентацию, [qw qx qy qz].

Примечание

Многие другие источники для 3-D графиков положения, включая .g2o форматы, задайте ориентацию кватерниона в другом порядке, например [qx qy qz qw]. Проверьте источник данных графика положения перед добавлением узлов к poseGraph3D объект.

Если на solver для входов задано значение "g2o-levenberg-marquardt":

свернуть все

Максимальное количество итераций, заданное как разделенная разделенными запятой парами, состоящая из 'MaxIterations' и положительное целое число. Оптимизатор выходит после того, как превысит это количество итераций.

Максимально допустимое время, заданное как разделенная разделенными запятой парами, состоящая из 'MaxTime' и положительный числовой скаляр в секундах. Оптимизатор выходит после того, как превысит это время.

Нижняя граница изменения функции затрат, заданная как разделенная разделенными запятой парами, состоящая из 'FunctionTolerance' и скаляром. Если изменение функции стоимости падает ниже этого значения между шагами оптимизации, оптимизатор выходит.

Отображение информации о промежуточной итерации в командной строке MATLAB, заданной как разделенная разделенными запятой парами, состоящая из 'VerboseOutput' и любой из них 'off' или 'on'.

Идентификаторы ребер замыкания цикла в poseGraph, заданная как разделенная разделенными запятой парами, состоящая из 'LoopClosuresToIgnore' и вектор. Чтобы получить идентификаторы ребер из графика положения, используйте findEdgeID.

Положение первого узла в poseGraph, заданная как разделенная разделенными запятой парами, состоящая из 'FirstNodePose' и вектор положения.

Для poseGraph (2-D), положение является [x y theta] вектор, который задает относительное xy -положение и угол ориентации, theta.

Для poseGraph3D, положение является [x y z qw qx qy qz] вектор, который определяет относительное xyz -положение и кватернионную ориентацию, [qw qx qy qz].

Примечание

Многие другие источники для 3-D графиков положения, включая .g2o форматы, задайте ориентацию кватерниона в другом порядке, например [qx qy qz qw]. Проверьте источник данных графика положения перед добавлением узлов к poseGraph3D объект.

Выходные аргументы

свернуть все

Оптимизированный 2-D или 3-D график положения, возвращенный как poseGraph или poseGraph3D объект.

Статистика процесса оптимизации, возвращенная как структура с этими полями:

  • Iterations - Количество итераций, используемых в оптимизации.

  • ResidualError - Значение функции стоимости при выходе оптимизатора.

  • Exit Flag - Выходное условие оптимизатора:

    • 1 - Локальный минимум найден.

    • 2 - Достигнуто максимальное количество итераций. См. MaxIterations аргумент пары "имя-значение".

    • 3 - Тайм-аут алгоритма во время работы.

    • 4 - Минимальный размер шага. Размер шага ниже StepTolerance аргумент пары "имя-значение".

    • 5 - Изменение ошибки ниже минимума.

    • 8 - Радиус доверительной области ниже минимального значения, установленного в InitialTrustRegionRadius.

Ссылки

[1] Grisetti, G., R. Kummerle, C. Stachniss, and W. Burgard. «Учебное руководство по SLAM на основе графа». Журнал интеллектуальных транспортных систем IEEE. Том 2, № 4, 2010, стр. 31-43. doi: 10.1109/mits.2010.939925.

[2] 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.

Расширенные возможности

.

См. также

Функции

Объекты

Темы

Введенный в R2019b

Документация по Navigation Toolbox

Поддержка