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

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

load grid-2d-posegraph.mat pg
disp(pg)

  poseGraph with properties:

               NumNodes: 120
               NumEdges: 193
    NumLoopClosureEdges: 74
     LoopClosureEdgeIDs: [120 121 122 123 124 125 126 127 128 129 130 ... ]
        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");