factorGraphSolverOptions

Solver options for factor graph

Since R2022a

Description

The factorGraphSolverOptions object contains solver options for optimizing a factor graph.

Creation

Syntax

Options = factorGraphSolverOptions

Options = factorGraphSolverOptions(Name=Value)

Description

Options = factorGraphSolverOptions returns a default factor graph solver options object, Options.

Options = factorGraphSolverOptions(Name=Value) specifies properties using one or more name-value arguments. For example, factorGraphSolverOptions(MaxIterations=150) sets the MaxIterations property of the factorGraphSolverOptions object to 150.

Properties

MaxIterations — Maximum number of solver iterations

200 (default) | positive integer

Maximum number of solver iterations, specified as a positive integer.

FunctionTolerance — Lower bound of change in cost function

1e-6 (default) | positive scalar

Lower bound of change in the cost function, specified as a positive scalar. The cost function is:

$$\left|newCost-oldCost\right|<FunctionTolerance*oldCost$$

All costs are greater than 0.

GradientTolerance — Lower bound of norm of gradient

1e-10 (default) | positive scalar

Lower bound of the norm of the gradient, specified as positive scalar. The norm function is:

$$max\_norm\{x-[x*Oplus-g(x)]\}<=GradientTolerance$$

Oplus is the manifold version of the plus operation and g(x) is the gradient at x.

StepTolerance — Lower bound of step size

1e-8 (default) | positive integer

Lower bound of step size of the linear solver, specified as a positive scalar. The relationship between the step size and the step tolerance is:

$$\left|deltaX\right|<=(\left|x\right|+StepTolerance)*StepTolerance$$

deltaX is the step size of the linear solver.

VerbosityLevel — Command line verbosity flag

0 (default) | 1 | 2

Command line verbosity flag, specified as 1, 2, or 3.

• 0 — Do not print to command line

• 1 — Print solver summary

• 2 — Print per-iteration updates and solver summary

TrustRegionStrategyType — Trust region step computation algorithm

1 (default) | 0

Trust region step computation algorithm, specified as 0 or 1.

• 0 — Levenberg Marquardt

• 1 — Dogleg

StateCovarianceType — Node types for node state covariance estimation

"none" (default) | "all-types" | "POSE_SE2" | "POSE_SE3" | "POINT_XY" | "POINT_XYZ" | "IMU_BIAS" | "VEL_3"

Node types for node state covariance estimation, specified as a string scalar, character vector, string array, or cell array of character vectors.

Specify "all-types" to estimate and store the node state covariance for all supported node types, and specify "none" to not estimate any node state covariance.

You can use a string array or cell array of character vectors to specify multiple node types for which to estimate and store node state covariance during factor graph optimization. Each element must be one of these options.

• "POSE_SE2" — Estimate and store node state covariance for nodes of type POSE_SE2.

• "POSE_SE3" — Estimate and store node state covariance for nodes of type POSE_SE3.

• "POINT_XY" — Estimate and store node state covariance for nodes of type POINT_XY.

• "POINT_XYZ" — Estimate and store node state covariance for nodes of type POINT_XYZ.

• "IMU_BIAS" — Estimate and store node state covariance for nodes of type IMU_BIAS.

• "VEL_3" — Estimate and store node state covariance for nodes of type VEL_3.

After optimizing the factor graph with StateCovarianceType set to a value other than "none", you can use the nodeCovariance function to get the node state covariance from the factor graph for any node types specified by StateCovarianceType.

Note

Optimization time increases the more node state covariances being estimated. If you have a factor graph with many nodes and node types, consider choosing only the necessary node types and using the sliding window optimization technique. See the Incrementally Optimize Factor Graph Using Sliding Window example for more information about the sliding window optimization technique.

Examples

Create and Optimize Factor Graph with Custom Options

Create and optimize a factor graph with custom solver options.

Create Factor Graph and Solver Settings

Create a factor graph and solver options with custom settings. Set the maximum number of iterations to 1000 and set the verbosity of the optimize output to 2.

G = factorGraph;
optns = factorGraphSolverOptions(MaxIterations=1000,VerbosityLevel=2)

optns = 
  factorGraphSolverOptions with properties:

              MaxIterations: 1000
          FunctionTolerance: 1.0000e-06
          GradientTolerance: 1.0000e-10
              StepTolerance: 1.0000e-08
             VerbosityLevel: 2
    TrustRegionStrategyType: 1
        StateCovarianceType: None

Add GPS Factor

Create a GPS factor with node identification number of 1 with NED ReferenceFrame and add it to the factor graph.

fgps = factorGPS(1,ReferenceFrame="NED");
addFactor(G,fgps);

Optimize Factor Graph

Optimize the factor graph with the custom settings. The results of the optimization are displayed with the level of detail depending on the VerbosityLevel.

optimize(G,optns);

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  0.000000e+00    0.00e+00    0.00e+00   0.00e+00   0.00e+00  1.00e+04        0    4.12e-04    4.85e-03
Terminating: Gradient tolerance reached. Gradient max norm: 0.000000e+00 <= 1.000000e-10

Solver Summary (v 2.0.0-eigen-(3.3.4)-no_lapack-eigensparse-no_openmp-no_custom_blas)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  7                        7
Effective parameters                        6                        6
Residual blocks                             1                        1
Residuals                                   3                        3

Minimizer                        TRUST_REGION

Sparse linear algebra library    EIGEN_SPARSE
Trust region strategy                  DOGLEG (TRADITIONAL)

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          0.000000e+00
Final                            0.000000e+00
Change                           0.000000e+00

Minimizer iterations                        1
Successful steps                            1
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.004438

  Residual only evaluation           0.000000 (0)
  Jacobian & residual evaluation     0.000383 (1)
  Linear solver                      0.000000 (0)
Minimizer                            0.002586

Postprocessor                        0.000004
Total                                0.007028

Termination:                      CONVERGENCE (Gradient tolerance reached. Gradient max norm: 0.000000e+00 <= 1.000000e-10)

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a

R2024b: Enable node state covariance estimation

The factor graph object can now estimate and store node state covariance during factor graph optimization. To enable this functionality, specify custom factor graph solver options and set the StateCovarianceType property to one or more node types for which to estimate and store state covariance. After optimizing with custom factor graph solver options, use the nodeCovariance function to get the stored state covariances from the factor graph.