Main Content

optimizePoses

Optimize absolute poses using relative pose constraints

Since R2020a

collapse all in page

Syntax

vSetOptim = optimizePoses(vSet)
[vSetOptim,poseScale] = optimizePoses(vSet)
vSetOptim = optimizePoses(vSet, minNumMatches)
vSetOptim = optimizePoses(___,Name,Value)

Description

vSetOptim = optimizePoses(vSet) returns an image view set whose absolute poses are optimized. vSetOptim and vSet are imageviewset 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.

example

[vSetOptim,poseScale] = optimizePoses(vSet) also returns the scales associated with the optimized absolute poses. This output applies only when the RelativePose of at least one connection is represented as a simtform3d object.

vSetOptim = optimizePoses(vSet, minNumMatches) additionally specifies the minimum number of matched feature points in a connection in order for the connection to be included in optimization.

vSetOptim = optimizePoses(___,Name,Value) uses additional options specified by one or more name-value arguments.

Examples

collapse all

Open Live Script

Create a view set.

vSet = imageviewset;

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

Image view set, specified as an imageviewset object.

Minimum number of connections, specified as a positive integer. Set minNumMatches to the minimum number of matched feature points in a connection for the connection to be included in optimization.

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

Maximum number of iterations before optimization is terminated, specified as a positive integer. Increase this value for more accurate results. Decrease this value for faster results.

Tolerance of the optimization cost function between two iterations, specified as a positive scalar. Optimization is terminated when the cost function changes by less than the Tolerance value between two iterations.

Display progress information, specified as a logical true or false. Set Verbose to true to display progress information.

Output Arguments

collapse all

Image view set that contains optimized absolute poses, returned as an imageviewset object.

Scales of optimized absolute poses, returned as a vector of positive values. This output applies only when the RelativePose of the pairwise Connections property, of at least one connection is represented as a simtform3d 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 factorization from the general (hyper) graph optimization (g2o) library, (1).

References

[1] Kuemmerle, R., G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard. "g2o: A General Framework for Graph Optimization IEEE International Conference on Robotics and Automation". Proceedings of the IEEE International Conference on Robotics and Automation, ICRA, 2011.

Extended Capabilities

Version History

Introduced in R2020a

expand all

See Also

Functions

Objects