Optimization¶
SSEOptimizer¶
-
namespace Tangent
-
template<typename ScalarType, typename Solver, typename Prior, typename Marginalizer, typename ...Variables, typename ...ErrorTerms>
class SSEOptimizer<Scalar<ScalarType>, Solver, Prior, Marginalizer, VariableGroup<Variables...>, ErrorTermGroup<ErrorTerms...>>¶ - #include <SSEOptimizer.h>
Sum of Squared Errors optimizer for manifold-based nonlinear least squares.
SSEOptimizer is a generic nonlinear optimizer capable of performing Levenberg-Marquardt or Gauss-Newton optimization on manifold. It maintains a Gaussian prior for all variables in its state, stored in a sparse block format to efficiently handle large state dimensionality while exploiting sparsity.
Designed primarily for windowed SLAM (Simultaneous Localization and Mapping) problems, SSEOptimizer supports:
Adding/removing variables and error terms dynamically
Marginalization of variables while preserving information through the Gaussian prior
Configurable optimization parameters (lambda, iterations, convergence thresholds)
During each iteration, the optimizer builds a linear system from the prior and linearized error terms to solve for a perturbation that minimizes the sum of squared errors:
(A_prior + J^T * W^(-1) * J) * dx = (b_prior + J^T * W^(-1) * e)
Note
Each variable type and error term type must be unique within the template parameters.
- Template Parameters:
ScalarType – The floating point type (typically float or double)
Solver – The linear system solver type (e.g., PSDSchurSolver)
Prior – The Gaussian prior type (e.g., GaussianPrior)
Marginalizer – The marginalization strategy type
Variables... – The variable types that can be optimized (must be unique types)
ErrorTerms... – The error term types (must be unique types)
Public Types
-
using ErrorTermContainerT = ErrorTermContainer<ErrorTerms...>¶
-
using MarginalizerT = Marginalizer¶
-
using ScalarT = ScalarType¶
Public Functions
-
SSEOptimizer() = default¶
Default constructor.
Uses a default-constructed solver. Note: This may not work if the Solver type requires initialization parameters.
-
inline explicit SSEOptimizer(Solver solverInstance)¶
Constructs the optimizer with a pre-configured solver.
-
template<typename ErrorTermType>
inline ErrorTermKey<ErrorTermType> addErrorTerm(const ErrorTermType &errorTerm)¶ Adds an error term to the problem.
-
template<typename VariableType>
inline VariableKey<VariableType> addVariable(VariableType &variable)¶ Inserts a variable into the optimizer with default weak prior.
-
template<typename VariableType>
inline VariableKey<VariableType> addVariable(VariableType &variable, const Eigen::Matrix<ScalarType, VariableType::dimension, VariableType::dimension> &informationMatrix)¶ Inserts a variable into the optimizer with a custom prior information matrix.
-
inline void clearErrorTerms()¶
Clears all error terms from the problem.
-
template<typename ErrorTermType>
inline size_t errorTermCount()¶ Returns the number of error terms of a specific type.
-
inline ErrorTermContainerT &getErrorTerms()¶
Gets reference to error term container.
-
inline const ErrorTermContainerT &getErrorTerms() const¶
-
inline Marginalizer &getMarginalizer()¶
Gets reference to internal marginalizer.
-
inline const Marginalizer &getMarginalizer() const¶
-
template<typename VariableType>
inline VariableType &getVariable(const VariableKey<VariableType> &key)¶ Gets a reference to a variable by key.
-
template<typename VariableType>
inline const VariableType &getVariable(const VariableKey<VariableType> &key) const¶ Gets a const reference to a variable by key.
-
template<typename VariableType>
inline std::vector<VariableKey<VariableType>> getVariableKeys()¶ Returns all keys for variables of a specific type.
-
inline VariableContainerT &getVariables()¶
Gets reference to variable container.
-
inline const VariableContainerT &getVariables() const¶
-
template<typename ErrorTermType>
inline bool hasErrorTerm(const ErrorTermKey<ErrorTermType> &key)¶ Checks if an error term exists.
-
template<typename VariableType>
inline bool hasVariable(const VariableKey<VariableType> &key)¶ Checks if a variable exists in the optimizer.
-
template<typename VariableType>
inline MarginalizationResult marginalizeVariable(VariableKey<VariableType> &key)¶ Marginalizes a variable, removing associated error terms and updating the prior.
Uses default residual threshold from settings.
-
template<typename VariableType, typename ...IgnoredVariables>
inline MarginalizationResult marginalizeVariable(VariableKey<VariableType> &key, VariableGroup<IgnoredVariables...> ignoredTypes)¶ Marginalizes a variable while ignoring correlations with specific variable types.
This is essential for preserving sparsity (e.g., ignore InverseDepth when marginalizing SE3).
-
template<typename VariableType, typename ...IgnoredVariables>
inline MarginalizationResult marginalizeVariable(VariableKey<VariableType> &key, VariableGroup<IgnoredVariables...> ignoredTypes, ScalarType residualThreshold)¶ Marginalizes a variable with custom residual threshold for outlier rejection.
-
inline OptimizationResult optimize()¶
Performs Levenberg-Marquardt optimization using current settings.
-
template<typename ErrorTermType>
inline void removeErrorTerm(const ErrorTermKey<ErrorTermType> &key)¶ Removes an error term from the problem.
-
template<typename VariableType>
inline void removeVariable(VariableKey<VariableType> &key)¶ Removes a variable from the problem without marginalizing it.
-
template<typename VariableType>
inline void setVariablePrior(const VariableKey<VariableType> &key, const Eigen::Matrix<ScalarType, VariableType::dimension, VariableType::dimension> &informationMatrix)¶ Updates the prior information matrix for an existing variable.
-
inline size_t totalDimension()¶
Returns the total dimension of all variables.
-
template<typename VariableType>
inline size_t variableCount()¶ Returns the number of variables of a specific type.
Public Members
-
ErrorTermContainer<ErrorTerms...> errorTerms¶
Error term container.
-
Marginalizer marginalizer¶
Marginalizer.
-
Settings settings¶
Optimizer settings.
-
template<>
struct MarginalizationResult¶ - #include <SSEOptimizer.h>
Result of a marginalization operation.
-
template<>
struct OptimizationResult¶ - #include <SSEOptimizer.h>
Result of an optimization run.
Public Members
-
bool converged = false¶
Whether convergence criteria were met.
-
bool errorDecreased = false¶
Whether final error < initial error.
-
std::vector<ScalarType> errorHistory¶
Error at each iteration.
-
ScalarType finalError = 0¶
Error after optimization.
-
ScalarType initialError = 0¶
Error before optimization.
-
size_t iterations = 0¶
Total iterations performed.
-
bool converged = false¶
-
template<>
struct Settings¶ - #include <SSEOptimizer.h>
Configuration settings for the optimizer.
Public Members
-
ScalarType errorDeltaThreshold = 0¶
-
ScalarType errorTermsPerThread = 1000¶
-
ScalarType initialLambda = 1e3¶
-
ScalarType lambdaReductionMultiplier = 10¶
-
ScalarType marginalizationResidualThreshold = std::numeric_limits<ScalarType>::max()¶
-
int maximumIterations = 50¶
-
bool parallelizeErrorTermLinearization = false¶
-
bool stopAfterErrorIncrease = false¶
-
ScalarType updateThreshold = 0¶
-
ScalarType errorDeltaThreshold = 0¶
-
template<typename ScalarType, typename Solver, typename Prior, typename Marginalizer, typename ...Variables, typename ...ErrorTerms>
Marginalizer¶
-
namespace Tangent
-
template<typename ScalarType, typename ...Variables, typename ...ErrorTerms>
class Marginalizer<Scalar<ScalarType>, VariableGroup<Variables...>, ErrorTermGroup<ErrorTerms...>>¶ - #include <Marginalizer.h>
Removes variables from the optimization problem while preserving information.
The Marginalizer approximates the effect of a variable’s error terms on the remaining variables by computing the Schur complement. When a variable is marginalized:
All error terms involving the variable are linearized at the current estimate
Their quadratic approximation (J^T * W * J) is added to the Gaussian prior
The Schur complement is computed to project information onto remaining variables
The marginalized variable and its error terms are removed
The Schur complement computes: A_remain = A_remain - B^T * inv(A_marg) * B where B represents the off-diagonal blocks connecting marginalized and remaining variables.
This is essential for sliding window estimation where old variables must be removed while preserving their constraining effect on the remaining state.
- Template Parameters:
ScalarType – The floating point type (typically float or double)
Variables... – The variable types in the optimization problem
ErrorTerms... – The error term types in the optimization problem
Public Types
-
template<typename RowVariable, typename ColumnVariable>
using MatrixArray = SlotArray<Eigen::Matrix<ScalarType, RowVariable::dimension, ColumnVariable::dimension>, VariableKey<ColumnVariable>>¶
-
template<typename RowVariable>
using Row = std::tuple<MatrixArray<RowVariable, Variables>...>¶
Public Functions
-
inline Marginalizer()¶
-
template<typename VariableType, typename ...IgnoredVariables>
inline bool marginalizeVariable(VariableKey<VariableType> &marginalizedKey, GaussianPrior<Scalar<ScalarType>, VariableGroup<Variables...>> &prior, ErrorTermContainer<ErrorTerms...> &linearizedErrorTerms, VariableGroup<IgnoredVariables...> ignoredVariableTypes = VariableGroup<IgnoredVariables...>(), ScalarType residualThreshold = std::numeric_limits<ScalarType>::max())¶ Marginalizes the variable requested using a set of linearized error terms.
It will be assumed that the error term jacobians and residual are good approximations of the error term. After marginalization, any error terms containing the variable will be erased. After marginalization, the prior should be cleared of all marginalized variables. Otherwise it will contain excess information.
Ignored variables, if already uncorrelated in the prior, will remain uncorrelated after marginalization. This is done by ignoring their off diagonal terms when computing the schur complement of the unmarginalized variables block.
- Parameters:
ignoredVariableTypes – Jacobians with respect to these variables types will be ignored.
marginalizedKey – This variable will be marginalized into the prior.
prior – The marginalized information will be added to this prior.
linearizedErrorTerms – These are the error terms which will be used to compute the marginalized information.
residualThreshold – The threshold used to determine if an error term should be marginalized into the prior or simply culled.
- Returns:
success flag signifying whether the marginalization was successful.
-
template<typename ScalarType, typename ...Variables, typename ...ErrorTerms>
PSDSchurSolver¶
-
namespace Tangent
- #include <PSDSchurSolver.h>
This class provides the functions to solve a levenberg marquardt or gauss newton iteration.
It sets up the sparse linear system, and solves it by exploiting the sparsity using the schur complement.
Internally, the linear system, which contains a square PSD matrix, is split into 3 blocks
\( \left[ \begin{array}{c|c} A & B \\ \hline B^{T} & D \end{array} \right] \)
A is a dense matrix, D is a block diagonal matrix, and B is a dense matrix correlating A and D.
Uncorrelated variables are variables which share no off diagonal elements. The uncorrelated variable set must be a subset of all variables.
Public Types
Public Functions
Adds a given column block to the left hand side of the problem.
This functions assumes that the index map has been computed.
Adds a given column block to the left hand side of the problem.
This functions assumes that the index map has been computed.
This adds a lambda to the diagonal members of A.
Ensures that the slot arrays stored in the solver are in sync with the current variable set.
Applies the perturbation to all variables using their box plus operator.
Assumes that the linear system has just been solved.
- Template Parameters:
revert – If true, this will apply the reverse update to the variables. This can be used to revert a negative update.
Using the set of linearized error terms, Build up a linear system by computing.
\( A = A_{0} + \sum_{i=0}^n J_{i}^{\top} \Sigma^{-1} J_{i} \rho \)
\( b = b_{0} + \sum_{i=0}^n J_{i}^{\top} \Sigma^{-1} e_{i} \rho \)
Where \( e_{i} \) is the residual and \( J_{i} \) is the jacobian of error w.r.t to all variables.
- Returns:
Whitened squared error. If no error terms were used, NaN is returned.
Computes the norm of the update vector.
Evaluates the error terms with the current variables.
Ensures that the update vector can be reverted if an update was bad.
- Parameters:
Variables – which will be updated.
This function is ran once before a solve.
This could be used to preallocate memory or precompute values.
Verifies that the update vector, dx, is valid.
Linearizes all error terms stored in this container.
Precomputes the map bewteen correlated variable keys and their index in A.
Ensures that the internal slot arrays do not have excess variables stored in them.
Sets the problem exactly to the prior.
\( A = A_{0} \)
\( b = b_{0} \)
Zeros all matrices in the problem.
This function will linearize the error terms if necessary, build the problem, and solve for the perturbation while exploiting the knowledge of the uncorrelated variable set.
After the solve, it can be assumed that the error terms are in the linearized state used for the final iteration.
Solves the linear system currently setup.
Warning the linear system is invalidated after this runs. It is assumed that the error terms are linearized.
From: https://en.wikipedia.org/wiki/Schur_complement
\( {\displaystyle M=\left[{\begin{matrix}A&B\\C&D\end{matrix}}\right]} \)
\( {\displaystyle M/D:=A-BD^{-1}C\,} \)
\( {\displaystyle M/A:=D-CA^{-1}B.} \)
\( {\displaystyle {\begin{aligned}&{\begin{bmatrix}A&B\\C&D\end{bmatrix}}^{-1}={\begin{bmatrix}I_{p}&0\\-D^{-1}C&I_{q}\end{bmatrix}}{\begin{bmatrix}\left(A-BD^{-1}C\right)^{-1}&0\\0&D^{-1}\end{bmatrix}}{\begin{bmatrix}I_{p}&-BD^{-1}\\0&I_{q}\end{bmatrix}}\\[4pt]={}&{\begin{bmatrix}\left(A-BD^{-1}C\right)^{-1}&-\left(A-BD^{-1}C\right)^{-1}BD^{-1}\\-D^{-1}C\left(A-BD^{-1}C\right)^{-1}&D^{-1}+D^{-1}C\left(A-BD^{-1}C\right)^{-1}BD^{-1}\end{bmatrix}}\\[4pt]={}&{\begin{bmatrix}\left(A-BD^{-1}C\right)^{-1}&-\left(A-BD^{-1}C\right)^{-1}BD^{-1}\\-D^{-1}C\left(A-BD^{-1}C\right)^{-1}&\left(D-CA^{-1}B\right)^{-1}\end{bmatrix}}\\[4pt]={}&{\begin{bmatrix}\left(M/D\right)^{-1}&-\left(M/D\right)^{-1}BD^{-1}\\-D^{-1}C\left(M/D\right)^{-1}&\left(M/A\right)^{-1}\end{bmatrix}}.\end{aligned}}} \)
Updates the pointers to variables inside the error terms.
Public Members
Dense matrix in the upper right corner.
Top right block matrix. Equal to bottom left transposed.
Preallocated rhs vector.
preallocated uncorreleted part of the b vector.
Block diagonal matrix in the bottom right. Each block is invertible.
The current dimension of the A matrix.
This exists because we do not need to reduce the size of A.
Pre allocated solution vector.
Loss function used to weight the error for each error term.
Used during the schur solve to store a precomputed: \( -B D^{-1} \).
The current total problem dimension.
Tuple of slot arrays which store the current index of a given variable in A.
- #include <PSDSchurSolver.h>
Public Members
Stopping condition based on the change in error.
elements per thread cost model.
This is used to determine how many threads to use for parallelization of error terms. This is meant to reduce the overhead of spawning n threads.
The first lambda used during the solve.
The value lambda is divided by each time a successful iteration occurs.
The maximum number of iterations for the solve.
Allow the solver to parallelize linearization.
Stop after the error increases.
Stopping condition based on the length of the update.
- #include <PSDSchurSolver.h>
Public Members
The error at each iteration.