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)

  poseGraph with properties:

               NumNodes: 1228
               NumEdges: 1483
    NumLoopClosureEdges: 256
     LoopClosureEdgeIDs: [1x256 double]

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')

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);

  poseGraph3D with properties:

               NumNodes: 1661
               NumEdges: 6275
    NumLoopClosureEdges: 4615
     LoopClosureEdgeIDs: [1x4615 double]

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)