Exponenta Banner
exponenta event banner

optimizePoseGraph

Оптимизация узлов в графе позы

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

Синтаксис

updatedGraph = optimizePoseGraph (poseGraph)
updatedGraph = optimizePoseGraph (poseGraph, решающее устройство)
[updatedGraph, solutionInfo] = optimizePoseGraph (___)
[___] = optimizePoseGraph (___, Имя, Стоимость)

Описание

пример

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

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

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

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

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

Примеры

свернуть все

Открыть сценарий в реальном времени

Оптимизируйте график позы на основе узлов и ограничений кромок. График позы, используемый в этом примере, взят из набора данных 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 объект. Вы должны иметь Toolbox™ компьютерного зрения и solver необходимо установить значение "builtin-trust-region". 'LoopClosuresToIgnore' и 'FirstNodePose' пары «имя-значение» игнорируются, если они указаны.

Края digraph объекты описаны affine3d(Панель инструментов обработки изображений) или rigid3d(Панель инструментов обработки изображений).

Решатель графов позы, указанный как "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] Гризетти, Г., Р. Куммерле, К. Стахнисс и У. Бургард. «Учебное пособие по SLAM на основе графиков». Журнал интеллектуальных транспортных систем IEEE. Том 2, № 4, 2010, стр. 31-43. doi: 10.1109/mits.2010.939925.

[2] Карлоне, Лука, Роберто Трон, Костас Даниилидис и Фрэнк Деллаерт. «Методы инициализации для 3D SLAM: исследование по оценке ротации и ее использованию в оптимизации графика позы». 2015 IEEE Международная конференция по робототехнике и автоматизации (ICRA). 2015, стр 4597–4604.

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

.

См. также

Функции

Объекты

Темы

Представлен в R2019b

Документация по панели инструментов навигации

Поддержка