File linearizer.h#

namespace sym

Functions

template<typename Scalar> SparseLinearization<Scalar> Linearize(const std::vector<Factor<Scalar>> &factors, const Values<Scalar> &values, const std::vector<Key> &keys_to_optimize = {}, const std::string &linearizer_name = "Linearizer")#: Free function as an alternate way to call the Linearizer

template<typename ScalarType> class Linearizer

#include <linearizer.h>

Class for evaluating multiple Factors at the linearization point given by a Values.

Stores the original Factors as well as the LinearizedFactors, and provides tools for aggregating keys and building a large jacobian / hessian for optimization.

For efficiency, prefer calling Relinearize() instead of re-constructing this object!

Public Types

using Scalar = ScalarType

using LinearizedDenseFactor = typename Factor<Scalar>::LinearizedDenseFactor

using LinearizedSparseFactor = typename Factor<Scalar>::LinearizedSparseFactor

using LinearizationType = SparseLinearization<Scalar>

Public Functions

Linearizer(const std::string &name, const std::vector<Factor<Scalar>> &factors, const std::vector<Key> &key_order = {}, bool include_jacobians = false, bool debug_checks = false)

Construct a Linearizer from factors and optional keys

Parameters:

factors – Only stores a pointer, MUST be in scope for the lifetime of this object!
key_order – If provided, acts as an ordered set of keys that form the state vector to optimize. Can equal the set of all factor keys or a subset of all factor keys. If not provided, it is computed from all keys for all factors using a default ordering.
debug_checks – Whether to perform additional sanity checks for NaNs. This uses additional compute but not additional memory except for logging.

void Relinearize(const Values<Scalar> &values, SparseLinearization<Scalar> &linearization)

Update linearization at a new evaluation point

This is more efficient than reconstructing this object repeatedly. On the first call, it will allocate memory and perform analysis needed for efficient repeated relinearization.

TODO(aaron): This should be const except that it can initialize the object

bool IsInitialized() const: Whether this contains values, versus having not been evaluated yet

const std::vector<LinearizedSparseFactor> &LinearizedSparseFactors() const

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

const std::unordered_map<key_t, index_entry_t> &StateIndex() const

Private Functions

void BuildInitialLinearization(const Values<Scalar> &values)#: Allocate all factor storage and compute sparsity pattern. This does a lot of index computation on the first linearization, such that repeated linearization can be fast.

void UpdateFromLinearizedDenseFactorIntoSparse(const LinearizedDenseFactor &linearized_factor, const linearization_dense_factor_helper_t &factor_helper, SparseLinearization<Scalar> &linearization) const#: Update the sparse combined problem linearization from a single factor.

void UpdateFromLinearizedSparseFactorIntoSparse(const LinearizedSparseFactor &linearized_factor, const linearization_sparse_factor_helper_t &factor_helper, SparseLinearization<Scalar> &linearization) const#

void UpdateFromDenseFactorIntoTripletLists(const LinearizedDenseFactor &linearized_factor, const linearization_dense_factor_helper_t &factor_helper, std::vector<Eigen::Triplet<Scalar>> &jacobian_triplets, std::vector<Eigen::Triplet<Scalar>> &hessian_lower_triplets) const#: Update the combined residual and rhs, along with triplet lists for the sparse matrices, from a single factor

void UpdateFromSparseFactorIntoTripletLists(const LinearizedSparseFactor &linearized_factor, const linearization_sparse_factor_helper_t &factor_helper, std::vector<Eigen::Triplet<Scalar>> &jacobian_triplets, std::vector<Eigen::Triplet<Scalar>> &hessian_lower_triplets) const#

void EnsureLinearizationHasCorrectSize(SparseLinearization<Scalar> &linearization) const#: Check if a Linearization has the correct sizes, and if not, initialize it

Private Members

bool initialized_ = {false}#

std::string name_#

const std::vector<Factor<Scalar>> *factors_#

std::vector<std::vector<index_entry_t>> factor_indices_#

bool include_jacobians_#

bool debug_checks_#

internal::LinearizedDenseFactorPool<Scalar> linearized_dense_factors_#

std::vector<LinearizedSparseFactor> linearized_sparse_factors_#

std::vector<Key> keys_#

std::unordered_map<key_t, index_entry_t> state_index_#

std::vector<linearization_dense_factor_helper_t> dense_factor_update_helpers_#

std::vector<linearization_sparse_factor_helper_t> sparse_factor_update_helpers_#

SparseLinearization<Scalar> init_linearization_#