# optimizePoseGraph

Optimize nodes in pose graph

## Syntax

``updatedGraph = optimizePoseGraph(poseGraph)``
``updatedGraph = optimizePoseGraph(poseGraph,solver)``
``[updatedGraph,solutionInfo] = optimizePoseGraph(___)``
``[___] = optimizePoseGraph(___,Name,Value)``

## Description

example

````updatedGraph = optimizePoseGraph(poseGraph)` adjusts the poses based on their edge constraints defined in the specified graph to improve the overall graph. You optimize either a 2-D or 3-D pose graph. The returned pose graph has the same topology with updated nodes. This pose graph optimization assumes all edge constraints and loop closures are valid. To consider trimming edges based on bad loop closures, see the `trimLoopClosures` function.```
````updatedGraph = optimizePoseGraph(poseGraph,solver)` specifies the solver type for optimizing the pose graph.```
````[updatedGraph,solutionInfo] = optimizePoseGraph(___)` returns additional statistics about the optimization process in `solutionInfo` using any of the previous syntaxes.```
````[___] = optimizePoseGraph(___,Name,Value)` specifies additional options using one or more `Name,Value` pairs. For example, `'MaxIterations',1000` increases the maximum number of iterations to 1000.```

## Examples

collapse all

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: [128 129 130 132 133 134 135 137 138 139 140 ... ] LandmarkNodeIDs: [1x0 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)```

## Input Arguments

collapse all

2-D or 3-D pose graph, specified as a `poseGraph`, `poseGraph3D`, `digraph` object.

To use the `digraph` object, generate the pose graph using `createPoseGraph` from an `imageviewset` or `pcviewset` object. You must have Computer Vision Toolbox™ and the `solver` must be set to `"builtin-trust-region"`. The `'LoopClosuresToIgnore'` and `'FirstNodePose'` name-value pairs are ignored if specified.

The edges of `digraph` object are described by `simtform3d` (Image Processing Toolbox) or `rigidtform3d` (Image Processing Toolbox) objects.

Pose graph solver, specified as either `"builtin-trust-region"` or `"g2o-levenberg-marquardt"`. To tune either solver, use the name-value pair arguments for that solver.

### Name-Value Arguments

Specify optional pairs of arguments as `Name1=Value1,...,NameN=ValueN`, where `Name` is the argument name and `Value` is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Before R2021a, use commas to separate each name and value, and enclose `Name` in quotes.

Example: `'MaxTime', 300`

Note

Depending on the `solver` input, the function supports different name-value pairs.

If the `solver` input is set to `"builtin-trust-region"`:

collapse all

Maximum time allowed, specified as the comma-separated pair consisting of `'MaxTime'` and a positive numeric scalar in seconds. The optimizer exits after it exceeds this time.

Lower bound on the norm of the gradient, specified as the comma-separated pair consisting of `'GradientTolerance'` and a scalar. The norm of the gradient is calculated based on the cost function of the optimization. If the norm falls below this value, the optimizer exits.

Lower bound on the change in the cost function, specified as the comma-separated pair consisting of `'FunctionTolerance'` and a scalar. If the cost function change falls below this value between optimization steps, the optimizer exits.

Lower bound on the step size, specified as the comma-separated pair consisting of `'StepTolerance'` and a scalar. If the norm of the optimization step falls below this value, the optimizer exits.

Initial trust region radius, specified as a scalar.

Display intermediate iteration information on the MATLAB® command line, specified as the comma-separated pair consisting of `'VerboseOutput'` and either `'off'` or `'on'`.

IDs of loop closure edges in `poseGraph`, specified as the comma-separated pair consisting of `'LoopClosuresToIgnore'` and a vector. To get edge IDs from the pose graph, use `findEdgeID`.

Pose of the first node in `poseGraph`, specified as the comma-separated pair consisting of `'FirstNodePose'` and a pose vector.

For `poseGraph` (2-D), the pose is an ```[x y theta]``` vector, which defines the relative xy-position and orientation angle, `theta`.

For `poseGraph3D`, the pose is an ```[x y z qw qx qy qz]``` vector, which defines the relative xyz-position and quaternion orientation, `[qw qx qy qz]`.

Note

Many other sources for 3-D pose graphs, including `.g2o` formats, specify the quaternion orientation in a different order, for example, ```[qx qy qz qw]```. Check the source of your pose graph data before adding nodes to your `poseGraph3D` object.

If the `solver` input is set to `"g2o-levenberg-marquardt"`:

collapse all

Maximum number of iterations, specified as the comma-separated pair consisting of `'MaxIterations'` and a positive integer. The optimizer exits after it exceeds this number of iterations.

Maximum time allowed, specified as the comma-separated pair consisting of `'MaxTime'` and a positive numeric scalar in seconds. The optimizer exits after it exceeds this time.

Lower bound on the change in the cost function, specified as the comma-separated pair consisting of `'FunctionTolerance'` and a scalar. If the cost function change falls below this value between optimization steps, the optimizer exits.

Display intermediate iteration information on the MATLAB command line, specified as the comma-separated pair consisting of `'VerboseOutput'` and either `'off'` or `'on'`.

IDs of loop closure edges in `poseGraph`, specified as the comma-separated pair consisting of `'LoopClosuresToIgnore'` and a vector. To get edge IDs from the pose graph, use `findEdgeID`.

Pose of the first node in `poseGraph`, specified as the comma-separated pair consisting of `'FirstNodePose'` and a pose vector.

For `poseGraph` (2-D), the pose is an ```[x y theta]``` vector, which defines the relative xy-position and orientation angle, `theta`.

For `poseGraph3D`, the pose is an ```[x y z qw qx qy qz]``` vector, which defines the relative xyz-position and quaternion orientation, `[qw qx qy qz]`.

Note

Many other sources for 3-D pose graphs, including `.g2o` formats, specify the quaternion orientation in a different order, for example, ```[qx qy qz qw]```. Check the source of your pose graph data before adding nodes to your `poseGraph3D` object.

## Output Arguments

collapse all

Optimized 2-D or 3-D pose graph, returned as a `poseGraph` or `poseGraph3D` object.

Statistics of optimization process, returned as a structure with these fields:

• `Iterations` — Number of iterations used in optimization.

• `ResidualError` — Value of cost function when optimizer exits.

• `Exit Flag` — Exit condition for optimizer:

• `1` — Local minimum found.

• `2` — Maximum number of iterations reached. See `MaxIterations` name-value pair argument.

• `3` — Algorithm timed out during operation.

• `4` — Minimum step size. The step size is below the `StepTolerance` name-value pair argument.

• `5` — The change in error is below the minimum.

• `8` — Trust region radius is below the minimum set in `InitialTrustRegionRadius`.

## References

[1] Grisetti, G., R. Kummerle, C. Stachniss, and W. Burgard. "A Tutorial on Graph-Based SLAM." IEEE Intelligent Transportation Systems Magazine. Vol. 2, No. 4, 2010, pp. 31–43. doi:10.1109/mits.2010.939925.

[2] Carlone, Luca, Roberto Tron, Kostas Daniilidis, and Frank Dellaert. "Initialization Techniques for 3D SLAM: a Survey on Rotation Estimation and its Use in Pose Graph Optimization." 2015 IEEE International Conference on Robotics and Automation (ICRA). 2015, pp. 4597–4604.

## Version History

Introduced in R2019b