trimLoopClosures

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

Описание

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

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

пример

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

Примеры

свернуть все

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

Загрузите набор данных, содержащий 2-D графики положения. Осмотрите 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 - Максимальное количество итераций, допустимых для обрезки замыкания цикла, заданное в виде положительного целого числа. За одну итерацию обрезки график положения оптимизируется на основе опций решателя и любых ребер за пределами 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] Yang, Heng, et al. «Выпускник Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection». IEEE Robotics and Automation Letters, vol. 5, № 2, Apr. 2020, pp. 1127-34. DOI.org (Crossref), doi:10.1109/LRA.2020.2965893.

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