Class sym::Optimizer

template<typename ScalarType, typename _NonlinearSolverType = LevenbergMarquardtSolver<ScalarType>>
class Optimizer

Class for optimizing a nonlinear least-squares problem specified as a list of Factors. For efficient use, create once and call Optimize() multiple times with different initial guesses, as long as the factors remain constant and the structure of the Values is identical.

Not thread safe! Create one per thread.

Example usage:

// Create a Values
sym::Key key0{'R', 0};
sym::Key key1{'R', 1};
sym::Valuesd values;
values.Set(key0, sym::Rot3d::Identity());
values.Set(key1, sym::Rot3d::Identity());

// Create some factors
std::vector<sym::Factord> factors;
factors.push_back(sym::Factord::Jacobian(
    [epsilon](const sym::Rot3d& rot, Eigen::Vector3d* const res,
              Eigen::Matrix3d* const jac) {
      const sym::Rot3d prior = sym::Rot3d::Random();
      const Eigen::Matrix3d sqrt_info = Eigen::Vector3d::Ones().asDiagonal();
      sym::PriorFactorRot3<double>(rot, prior, sqrt_info, epsilon, res, jac);
    },
    {key0}));
factors.push_back(sym::Factord::Jacobian(
    [epsilon](const sym::Rot3d& rot, Eigen::Vector3d* const res,
              Eigen::Matrix3d* const jac) {
      const sym::Rot3d prior = sym::Rot3d::Random();
      const Eigen::Matrix3d sqrt_info = Eigen::Vector3d::Ones().asDiagonal();
      sym::PriorFactorRot3<double>(rot, prior, sqrt_info, epsilon, res, jac);
    },
    {key1}));
factors.push_back(sym::Factord::Jacobian(
    [epsilon](const sym::Rot3d& a, const sym::Rot3d& b, Eigen::Vector3d* const res,
              Eigen::Matrix<double, 3, 6>* const jac) {
      const Eigen::Matrix3d sqrt_info = Eigen::Vector3d::Ones().asDiagonal();
      const sym::Rot3d a_T_b = sym::Rot3d::Random();
      sym::BetweenFactorRot3<double>(a, b, a_T_b, sqrt_info, epsilon, res, jac);
    },
    {key0, key1}));

// Set up the params
sym::optimizer_params_t params = DefaultLmParams();
params.iterations = 50;
params.early_exit_min_reduction = 0.0001;

// Optimize
sym::Optimizer<double> optimizer(params, factors, epsilon);
optimizer.Optimize(values);

See symforce/test/symforce_optimizer_test.cc for more examples

Public Types

using Scalar = ScalarType

using NonlinearSolverType = _NonlinearSolverType

using FailureReason = typename NonlinearSolverType::FailureReason

using MatrixType = typename NonlinearSolverType::MatrixType

using Stats = OptimizationStats<MatrixType>

using LinearizerType = internal::LinearizerSelector_t<MatrixType>

Public Functions

Optimizer(const optimizer_params_t &params, std::vector<Factor<Scalar>> factors, const std::string &name = "sym::Optimize", std::vector<Key> keys = {}, const Scalar epsilon = sym::kDefaultEpsilon<Scalar>)

Base constructor

Parameters:

• params – The params to use for the optimizer and nonlinear solver

• factors – The set of factors to include

• name – The name of this optimizer to be used for printing debug information

• keys – The set of keys to optimize. If empty, will use all optimized keys touched by the factors

• epsilon – Epsilon for numerical stability

Optimizer(const optimizer_params_t &params, std::vector<Factor<Scalar>> factors, const Scalar epsilon, const std::string &name, std::vector<Key> keys, bool debug_stats, bool check_derivatives = false, bool include_jacobians = false)

template<typename ...NonlinearSolverArgs>
Optimizer(const optimizer_params_t &params, std::vector<Factor<Scalar>> factors, const std::string &name, std::vector<Key> keys, Scalar epsilon, NonlinearSolverArgs&&... nonlinear_solver_args)

Constructor that copies in factors and keys, with arguments for the nonlinear solver

See the base constructor for argument descriptions, additional arguments are forwarded to the constructor for NonlinearSolverType

template<typename ...NonlinearSolverArgs>
Optimizer(const optimizer_params_t &params, std::vector<Factor<Scalar>> factors, Scalar epsilon, const std::string &name, std::vector<Key> keys, bool debug_stats, bool check_derivatives, bool include_jacobians, NonlinearSolverArgs&&... nonlinear_solver_args)

Optimizer(Optimizer&&) = delete

Optimizer &operator=(Optimizer&&) = delete

Optimizer(const Optimizer&) = delete

Optimizer &operator=(const Optimizer&) = delete

virtual ~Optimizer() = default

Stats Optimize(Values<Scalar> &values, int num_iterations = -1, bool populate_best_linearization = false)

Optimize the given values in-place

Parameters:

• num_iterations – If < 0 (the default), uses the number of iterations specified by the params at construction

• populate_best_linearization – If true, the linearization at the best values will be filled out in the stats

Returns:

The optimization stats

virtual void Optimize(Values<Scalar> &values, int num_iterations, bool populate_best_linearization, Stats &stats)

Optimize the given values in-place

This overload takes the stats as an argument, and stores into there. This allows users to avoid reallocating memory for any of the entries in the stats, for use cases where that’s important.

Parameters:

• num_iterations – If < 0 (the default), uses the number of iterations specified by the params at construction

• populate_best_linearization – If true, the linearization at the best values will be filled out in the stats

• stats – An OptimizationStats to fill out with the result - if filling out dynamically allocated fields here, will not reallocate if memory is already allocated in the required shape (e.g. for repeated calls to Optimize())

void Optimize(Values<Scalar> &values, int num_iterations, Stats &stats)

Optimize the given values in-place

This overload takes the stats as an argument, and stores into there. This allows users to avoid reallocating memory for any of the entries in the stats, for use cases where that’s important.

Parameters:

• num_iterations – If < 0 (the default), uses the number of iterations specified by the params at construction

• stats – An OptimizationStats to fill out with the result - if filling out dynamically allocated fields here, will not reallocate if memory is already allocated in the required shape (e.g. for repeated calls to Optimize())

void Optimize(Values<Scalar> &values, Stats &stats)

Optimize the given values in-place

This overload takes the stats as an argument, and stores into there. This allows users to avoid reallocating memory for any of the entries in the stats, for use cases where that’s important.

Parameters:

stats – An OptimizationStats to fill out with the result - if filling out dynamically allocated fields here, will not reallocate if memory is already allocated in the required shape (e.g. for repeated calls to Optimize())

Linearization<MatrixType> Linearize(const Values<Scalar> &values)

Linearize the problem around the given values

void ComputeAllCovariances(const Linearization<MatrixType> &linearization, std::unordered_map<Key, MatrixX<Scalar>> &covariances_by_key)

Get covariances for each optimized key at the given linearization

Will reuse entries in covariances_by_key, allocating new entries so that the result contains exactly the set of keys optimized by this Optimizer. covariances_by_key must not contain any keys that are not optimized by this Optimizer.

May not be called before either Optimize() or Linearize() has been called.

void ComputeCovariances(const Linearization<MatrixType> &linearization, const std::vector<Key> &keys, std::unordered_map<Key, MatrixX<Scalar>> &covariances_by_key)

Get covariances for the given subset of keys at the given linearization

This version is potentially much more efficient than computing the covariances for all keys in the problem.

Currently requires that keys corresponds to a set of keys at the start of the list of keys for the full problem, and in the same order. It uses the Schur complement trick, so will be most efficient if the hessian is of the following form, with C block diagonal:

A = ( B    E )
    ( E^T  C )

Will reuse entries in covariances_by_key, allocating new entries so that the result contains exactly the set of keys requested. covariances_by_key must not contain any keys that are not in keys.

void ComputeFullCovariance(const Linearization<MatrixType> &linearization, MatrixX<Scalar> &covariance)

Get the full problem covariance at the given linearization

Unlike ComputeCovariance and ComputeAllCovariances, this includes the off-diagonal blocks, i.e. the cross-covariances between different keys.

The ordering of entries here is the same as the ordering of the keys in the linearization, which can be accessed via optimizer.Linearizer().StateIndex().

May not be called before either Optimize() or Linearize() has been called.

Parameters:

covariance – A matrix that will be filled out with the full problem covariance.

const std::vector<Key> &Keys() const

Get the optimized keys

const std::vector<Factor<Scalar>> &Factors() const

Get the factors

const LinearizerType &Linearizer() const

Get the Linearizer object

LinearizerType &Linearizer()

const NonlinearSolverType &NonlinearSolver() const

Get the NonlinearSolver object

NonlinearSolverType &NonlinearSolver()

void UpdateParams(const optimizer_params_t &params)

Update the optimizer params

const optimizer_params_t &Params() const

Get the params used by the nonlinear solver