%% Optimize a 2-D Pose Graph
% Optimize a pose graph based on the nodes and edge constraints. The pose graph used in this example is from the Intel Research Lab Dataset and was generated from collecting wheel odometry and a laser range finder sensor information in an indoor lab.
%
% Load the Intel data set that contains a 2-D pose graph. Inspect the poseGraph object to view the number of nodes and loop closures.

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

%
% Plot the pose graph with IDs off. Red lines indicate loop closures identified in the dataset.

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

%
% Optimize the pose graph. Nodes are adjusted based on the edge constraints and loop closures. Plot the optimized pose graph to see the adjustment of the nodes with loop closures.

updatedPG = optimizePoseGraph(pg);
figure
show(updatedPG,'IDs','off');
title('Updated Pose Graph')

% Copyright 2019 The MathWorks, Inc.