# optimizePoses

Optimize absolute poses using relative pose constraints

Since R2020a

## Syntax

``vSetOptim = optimizePoses(vSet)``
``vSetOptim = optimizePoses(vSet,Name,Value)``

## Description

example

````vSetOptim = optimizePoses(vSet)` returns a point cloud view set whose absolute poses are optimized. `vSetOptim` and `vSet` are `pcviewset` objects.The `optimizePoses` function performs pose graph optimization on the absolute poses for the `Views` in the view set using the relative pose constraints established by the `Connections` property. You can use `optimizePoses` to correct drift in odometry after detecting loop closures.```
````vSetOptim = optimizePoses(vSet,Name,Value)` specifies options using one or more name-value pair arguments. For example, `'Tolerance'`,`0.2` sets the tolerance of the optimization cost function to `0.2`.```

## Examples

collapse all

Create a view set.

`vSet = pcviewset;`

Add four nodes and specify absolute poses.

```absPoses = repelem(rigidtform3d,4,1); absPoses(1).Translation = [0 0 0]; absPoses(2).Translation = [1 0 0]; absPoses(3).Translation = [2 0 0]; absPoses(4).Translation = [0.1 0 0]; vSet = addView(vSet,1,absPoses(1)); vSet = addView(vSet,2,absPoses(2)); vSet = addView(vSet,3,absPoses(3)); vSet = addView(vSet,4,absPoses(4));```

Define 4 edges, 3 odometry and 1 loop closure.

```relPoses = repelem(rigidtform3d,4,1); relPoses(1).Translation = [1 0 0]; relPoses(2).Translation = [1 0 0]; relPoses(3).Translation = [-1.9 0 0]; relPoses(4).Translation = [0.2 0 0]; vSet = addConnection(vSet,1,2,relPoses(1)); % odometry vSet = addConnection(vSet,2,3,relPoses(2)); % odometry vSet = addConnection(vSet,3,4,relPoses(3)); % odometry vSet = addConnection(vSet,4,1,relPoses(4)); % loop closure```

Optimize view set.

`vSetOptim = optimizePoses(vSet);`

DIsplay original and optimized locations.

`disp('Original absolute translations:')`
```Original absolute translations: ```
`disp(vertcat(vSet.Views.AbsolutePose.Translation))`
``` 0 0 0 1.0000 0 0 2.0000 0 0 0.1000 0 0 ```
`disp('Optimized absolute translations:')`
```Optimized absolute translations: ```
`disp(vertcat(vSetOptim.Views.AbsolutePose.Translation))`
``` 0 0 0 0.9250 0 0 1.8500 0 0 -0.1250 0 0 ```

## Input Arguments

collapse all

Point cloud view set, specified as a `pcviewset` object.

### 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: '`MaxIterations`',`300` sets the maximum number of iterations to `300`.

Maximum number of iterations before the function terminates optimization, specified as the comma-separated pair consisting of `'MaxIterations'` and a positive integer. Increase this value for greater accuracy in the results. Decrease this value for faster results.

Tolerance of the optimization cost function between two consecutive iterations, specified as the comma-separated pair consisting of '`Tolerance`' and a positive scalar. If the cost function changes by less than the '`Tolerance`' value between two consecutive iterations, the function terminates optimization.

Display progress information, specified as the comma-separated pair consisting of `Verbose` and a numeric or logical `0` (`false`) or `1` (`true`). To display the progress information, set '`Verbose`' to `true`.

## Output Arguments

collapse all

Point cloud view set that contains optimized absolute poses, specified as a `pcviewset` object.

## Tips

• To update a view set with optimized poses, use the `updateView` object function.

• The `optimizePoses` object function holds the first view fixed.

## Algorithms

The `optimizePoses` function uses the Levenberg-Marquardt optimization algorithm with sparse Cholesky decomposition from the general (hyper) graph optimization (G2o) library, [1].

## References

[1] Kümmerle, Rainer, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard. “G2o: A General Framework for Graph Optimization.” In 2011 IEEE International Conference on Robotics and Automation, 3607–13, 2011. https://doi.org/10.1109/ICRA.2011.5979949.

## Version History

Introduced in R2020a