%% Optimize a 3-D Pose Graph
% Optimize a pose graph based on the nodes and edge constraints. The pose graph used in this example is taken from the MIT Dataset and was generated using information extracted from a parking garage.
%
% Load the pose graph from the MIT dataset. Inspect the poseGraph3D object to view the number of nodes and loop closures.

load parking-garage-posegraph.mat pg
disp(pg);

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

title('Original Pose Graph')
show(pg,'IDs','off');
view(-30,45)

%
% 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
title('Updated Pose Graph')
show(updatedPG,'IDs','off');
view(-30,45)

% Copyright 2019 The MathWorks, Inc.