File linearizer.h#

namespace sym


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

  • 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_#