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

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