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

Загрузите набор данных, содержащий 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')

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

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

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

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

Некоторые замыкания цикла должны быть отделены от графика положения на основе их остаточной ошибки. Используйте 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

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

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