Exponenta Banner

optimizePoseGraph

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

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

Синтаксис

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

Описание

пример

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

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

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

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

[___] = optimizePoseGraph(___,Name,Value) задает дополнительные опции с помощью одного или нескольких Name,Value пары. Например, 'MaxIterations',1000 увеличивает максимальное число итераций к 1 000.

Примеры

свернуть все

Скрипт Open Live Script

Оптимизируйте график положения на основе ограничений ребра и узлов. График положения, используемый в этом примере, взят из Набора данных MIT и был сгенерирован с помощью информации, извлеченной из гаража.

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

load parking-garage-posegraph.mat pg
disp(pg);
  poseGraph3D with properties:

               NumNodes: 1661
               NumEdges: 6275
    NumLoopClosureEdges: 4615
     LoopClosureEdgeIDs: [128 129 130 132 133 134 135 137 138 139 140 ... ]
        LandmarkNodeIDs: [1x0 double]

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

title('Original Pose Graph')
show(pg,'IDs','off');
view(-30,45)

Figure contains an axes object. The axes object 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 object. The axes object contains 3 objects of type line.

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

свернуть все

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

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

Ребра digraph объект описан affine3d (Image Processing Toolbox) или rigid3d (Image Processing Toolbox) объекты.

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

Аргументы name-value

Задайте дополнительные разделенные запятой пары 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 (2D), положением является [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 (2D), положением является [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 объект.

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

свернуть все

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

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

  • Iterations — Количество итераций используется в оптимизации.

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

  • Exit Flag — Выйдите из условия для оптимизатора:

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

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

    • 3 — Алгоритм приведен к таймауту во время операции.

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

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

    • 8 — Доверительный радиус области ниже минимального набора в InitialTrustRegionRadius.

Ссылки

[1] Гризетти, G., Р. Каммерл, К. Стэкнисс и В. Бергард. "Пример на Основанном на графике SLAM". IEEE Интеллектуальный Системный Журнал Транспортировки. Издание 2, № 4, 2010, стр 31–43. doi:10.1109/mits.2010.939925.

[2] Carlone, Лука, Роберто Трон, Костас Даниилидис и Франк Деллэерт. "Методы инициализации для 3D SLAM: Обзор Оценки Вращения и ее Использования в Оптимизации Графика положения". 2 015 Международных конференций IEEE по вопросам Робототехники и Автоматизации (ICRA). 2015, стр 4597–4604.

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

Смотрите также

Функции

Объекты

Темы

Введенный в R2019b

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

Поддержка