trimLoopClosures

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

Описание

poseGraphUpdated = trimLoopClosures(poseGraphObj,trimParams,solverOptions) оптимизирует график положения, чтобы лучше всего удовлетворить, ребро ограничивает и удаляет любые плохие ребра закрытия цикла на основе параметров остаточной ошибки, заданных в trimParams. Создайте solverOptions введите использование poseGraphSolverOptions функция.

Функция реализует метод дипломированной невыпуклости (GNC) с усеченными наименьшими квадратами (TLS) устойчивая стоимость в сочетании с неминимальным решателем графика положения [1].

пример

[poseGraphUpdated,trimInfo] = trimLoopClosures(poseGraphObj,trimParams,solverOptions) возвращает дополнительную информацию, связанную с процессом обрезки.

Примеры

свернуть все

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

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

load grid-2d-posegraph.mat pg
disp(pg)
  poseGraph with properties:

               NumNodes: 120
               NumEdges: 193
    NumLoopClosureEdges: 74
     LoopClosureEdgeIDs: [1x74 double]
        LandmarkNodeIDs: [1x0 double]

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

show(pg,'IDs','off');
title('Original Pose Graph')

Figure contains an axes. The axes with title Original Pose Graph contains 3 objects of type line.

Оптимизируйте график положения с помощью optimizePoseGraph функция. По умолчанию эта функция использует "builtin-trust-region" решатель. Поскольку график положения содержит некоторые плохие закрытия цикла, получившийся график положения является фактическим не желательный.

pgOptim = optimizePoseGraph(pg);
figure;
show(pgOptim);

Figure contains an axes. The axes contains 225 objects of type line, text.

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

resErrorVec = edgeResidualErrors(pg);
plot(resErrorVec);
title('Edge Residual Errors by Edge ID')

Figure contains an axes. The axes with title Edge Residual Errors by Edge ID contains an object of type line.

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

trimParams.MaxIterations = 100;
trimParams.TruncationThreshold = 25;

solverOptions = poseGraphSolverOptions; 

Используйте trimLoopClosures функция параметрами более для обрезки и опциями решателя.

[pgNew, trimInfo, debugInfo] = trimLoopClosures(pg,trimParams,solverOptions);

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

removedLCs = trimInfo.LoopClosuresToRemove;

hold on
plot(removedLCs,zeros(length(removedLCs)),'or')
title('Edge Residual Errors and Removed Loop Closures')
legend('Residual Errors', 'Removed Loop Closures')
xlabel('Edge IDs')
ylabel('Edge Residual Error')
hold off

Figure contains an axes. The axes with title Edge Residual Errors and Removed Loop Closures contains 45 objects of type line. These objects represent Residual Errors, Removed Loop Closures.

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

show(pgNew,"IDs","off");

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

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

свернуть все

График положения в виде poseGraph или poseGraph3D объект.

Параметры остаточной ошибки для обрезки закрытий цикла в виде структуры с полями:

  • MaxIterations — Максимальное количество итераций позволило обрезку закрытия цикла for в виде положительного целого числа. В одной итерации обрезки график положения оптимизирован на основе опций решателя и любых ребер вне TruncationThreshold обрезаются.

  • TruncationThreshold — Максимальная позволенная остаточная ошибка для ребра. Это значение зависит в большой степени от графика положения, который вы задаете в poseGraphObj. Чтобы найти соответствующий порог на основе всех ошибок, используйте edgeResidualErrors функция для графика положения.

Пример: struct('MaxIterations',10,'TruncationThreshold',20)

Типы данных: struct

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

pgSolverTrustRegion = poseGraphSolverOptions('builtin-trust-region')
pgSolverTrustRegion = 

TrustRegion (builtin-trust-region-dogleg) options:

               MaxIterations: 300
                     MaxTime: 10
           FunctionTolerance: 1.0000e-08
           GradientTolerance: 5.0000e-09
               StepTolerance: 1.0000e-12
    InitialTrustRegionRadius: 100
               VerboseOutput: 'off'
pgSolverG2o = poseGraphSolverOptions('g2o-levenberg-marquardt')
pgSolverG2o = 

G2oLevenbergMarquardt (g2o-levenberg-marquardt) options:

        MaxIterations: 300
              MaxTime: 10
    FunctionTolerance: 1.0000e-09
        VerboseOutput: 'off'

Измените опции, чтобы настроить параметры решателя с помощью записи через точку.

pgSolverG2o.MaxIterations = 200;

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

свернуть все

График положения с обрезанными циклично выполненными закрытиями в виде poseGraph или poseGraph3D объект.

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

  • LoopClosuresToRemove — Идентификаторы ребра закрытия цикла, чтобы удалить из входа poseGraphObj. Эти закрытия цикла удалены в выходе poseGraphUpdated.

  • Iterations — Количество обрезки итераций выполняется.

Ссылки

[1] Ян, Хэн, и др. “Дипломированная Невыпуклость для Устойчивого Пространственного Восприятия: От Неминимальных Решателей до Глобального Отклонения Выброса”. Робототехника IEEE и Буквы Автоматизации, издание 5, № 2, апрель 2020, стр 1127–34. DOI.org (Crossref), doi:10.1109/LRA.2020.2965893.

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

Функции

Объекты

Введенный в R2020b
Для просмотра документации необходимо авторизоваться на сайте