Types¶

SparseBlockMatrix¶

namespace Tangent

template<typename ScalarType, typename ...Variables> class SparseBlockMatrix<Scalar<ScalarType>, VariableGroup<Variables...>>¶

#include <SparseBlockMatrix.h>

Sparse block matrix indexed by variable keys.

Stores matrix blocks indexed by VariableKeys, organized as a collection of SparseBlockRows. Each row corresponds to a variable and contains blocks for non-zero column entries. Blocks are accessed and modified using variable key pairs (row, column).

Template Parameters:

ScalarType – The floating point type (typically float or double)
Variables... – The variable types that can index this matrix

Public Types

template<typename VariableType> using Row = SparseBlockRow<Scalar<ScalarType>, Dimension<VariableType::dimension>, VariableGroup<Variables...>>¶

template<typename VariableType> using RowMap = std::map<VariableKey<VariableType>, Row<VariableType>>¶

Public Functions

inline SparseBlockMatrix()¶

template<typename VariableType> inline auto &addRowIfItDoesNotExist(VariableKey<VariableType> key)¶: Inserts a row into the matrix and return its reference.

template<int DenseMatrixColumns> inline void dot(VariableContainer<Variables...> &variableOrder, const Eigen::Matrix<ScalarType, Eigen::Dynamic, DenseMatrixColumns> &v, Eigen::Matrix<ScalarType, Eigen::Dynamic, DenseMatrixColumns> &result)¶

Computes the dot product of this Matrix with a dense column vector.

The variable container is used to determine the indices of each block. a.k.a. A * v = result

template<typename RowType, typename ColType> inline Eigen::Matrix<ScalarType, RowType::dimension, ColType::dimension> &getBlock(VariableKey<RowType> rowKey, VariableKey<ColType> colKey)¶

Gets a block matrix reference from the matrix.

If the block does not exist, it is inserted and set to zero.

Parameters:

rowKey – key pointing to the row.
columnKey – key pointing to the column.

template<typename VariableType> inline RowMap<VariableType> &getRowMap()¶: Gets the map of rows for a given variable type.

template<typename RowType, typename ColType> inline void removeBlock(VariableKey<RowType> rowKey, VariableKey<ColType> colKey)¶: Removes a block using the keys. Essentially, sets the block to zero.

template<typename RowType, typename ColType> inline void setBlock(VariableKey<RowType> rowKey, VariableKey<ColType> colKey, const Eigen::Matrix<ScalarType, RowType::dimension, ColType::dimension> &blockMatrix)¶: Inserts or assigns a block at the given keys.

Private Members

std::tuple<RowMap<Variables>...> tupleOfRowMaps¶

BlockVector¶

namespace Tangent

template<typename ScalarType, int ColumnDimension, typename ...Variables> class BlockVector<Scalar<ScalarType>, Dimension<ColumnDimension>, VariableGroup<Variables...>>¶

#include <BlockVector.h>

Unlike the SparseBlockRow, the BlockVector is assumed to be dense and can then achieve constant time insert, lookup, and erase via the slot map.

This class uses a slightly different variant of the slot map. One which allows us to choose a key value pair.

Public Types

template<typename VariableType> using MatrixBlock = Eigen::Matrix<ScalarType, VariableType::dimension, ColumnDimension>¶

template<typename VariableType> using RowMap = SlotArray<MatrixBlock<VariableType>, VariableKey<VariableType>>¶

Public Functions

inline BlockVector()¶

template<typename VariableType> inline RowMap<VariableType>::InsertResult addRowBlock(VariableKey<VariableType> key, MatrixBlock<VariableType> &value)¶

Inserts a key value pair into the slot map.

This should never need overwrite an element.

template<typename VariableType> inline bool blockExists(VariableKey<VariableType> key)¶: Evaluates if the key exists in the block vector.

template<typename VariableType> inline MatrixBlock<VariableType> &getRowBlock(VariableKey<VariableType> key)¶: Gets a reference to the block matrix at the key.

template<typename VariableType> inline RowMap<VariableType> &getRowMap()¶: Gets a reference to the internal row map for a given variable type.

template<typename VariableType> inline const RowMap<VariableType> &getRowMap() const¶: Gets a const reference to the internal row map for a given variable type.

template<typename VariableType> inline void removeRowBlock(VariableKey<VariableType> key)¶: Removes the row block.

inline void subtractVector(VariableContainer<Variables...> &variableOrder, const Eigen::Matrix<ScalarType, Eigen::Dynamic, ColumnDimension> &v)¶: Computes BlockVector -= v.

Private Members

std::tuple<RowMap<Variables>...> tupleOfRowMaps¶

GaussianPrior¶

namespace Tangent

template<typename ScalarType, typename ...Variables> class GaussianPrior<Scalar<ScalarType>, VariableGroup<Variables...>>¶

#include <GaussianPrior.h>

Represents a Gaussian prior over the optimizer’s variables.

The GaussianPrior stores the accumulated information from marginalized variables and error terms as a quadratic cost function. It is represented as:

A0: Sparse block information matrix (inverse covariance)
b0: Dense mean vector

The prior contributes to the optimization objective as: 0.5 * x^T * A0 * x - b0^T * x

During marginalization, when a variable is removed from the problem, its contribution to the remaining variables is approximated and added to this Gaussian prior via the Schur complement. The sparse block format allows efficient handling of large state dimensions while exploiting the inherent sparsity in SLAM problems.

After each optimization iteration, the prior must be updated on-manifold to account for the variable perturbations: A0 * (x + dx) = b0 becomes A0 * x = b0 - A0 * dx

Template Parameters:

ScalarType – The floating point type (typically float or double)
Variables... – The variable types this prior can contain

Public Types

using BV = BlockVector<Scalar<ScalarType>, Dimension<1>, VariableGroup<Variables...>>¶

using SBM = SparseBlockMatrix<Scalar<ScalarType>, VariableGroup<Variables...>>¶

Public Functions

inline GaussianPrior()¶

template<typename VariableType> inline void addVariable(VariableKey<VariableType> &key, Eigen::Matrix<ScalarType, VariableType::dimension, VariableType::dimension> informationMatrix = Eigen::Matrix<ScalarType, VariableType::dimension, 1>::Constant(DefaultInverseVariance).asDiagonal())¶: Adds variable to the gaussian prior with an initial uncertainty.

inline void removeUnsedVariables(VariableContainer<Variables...> &variableContainer)¶

Remove all variables from the gaussian prior which are not part of the given set of variables.

This operation has a complexity of N^2 where N is the number of elements in a row. This is the worst case and will typically be much lower since the prior is typically very sparse.

inline void update(BlockVector<Scalar<ScalarType>, Dimension<1>, VariableGroup<Variables...>> &dx)¶

Updates the prior with a block vector.

The block vector must contain at least a subset of the variables contained in the b0 vector. This is the most efficient variant of this operation and is agnostic to ordering.

inline void update(VariableContainer<Variables...> &variableOrder, const Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> &dx)¶

Updates the prior error term on manifold.

A0 * (x + dx) = b0 => A0 * x = b0 - A0 * dx; Assumes that the dx vector has the same variable order as the variable container.

Public Members

SBM A0¶: Sparse block information matrix representing the prior uncertainty.

BV b0¶: Dense mean vector of the Gaussian prior.

Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> temporaryVector¶: Preallocated temporary vector for update computations.

Public Static Attributes

static constexpr ScalarType DefaultInverseVariance = 1e-24¶

SparseBlockRow¶

namespace Tangent

template<typename ScalarType, int RowDimension, typename ...Variables> class SparseBlockRow<Scalar<ScalarType>, Dimension<RowDimension>, VariableGroup<Variables...>>¶

#include <SparseBlockRow.h>

A single row of a SparseBlockMatrix, containing blocks indexed by variable keys.

Stores matrix blocks as a tuple of maps, one for each variable type. Each map contains blocks indexed by VariableKey. Supports dot product operations with dense vectors and BlockVectors.

Template Parameters:

ScalarType – The floating point type
RowDimension – The number of rows in each block (fixed at compile time)
Variables... – The variable types whose keys can index columns

Public Types

template<typename VariableType> using MatrixBlock = Eigen::Matrix<ScalarType, RowDimension, VariableType::dimension>¶

template<typename VariableType> using VariableColumns = std::map<VariableKey<VariableType>, MatrixBlock<VariableType>>¶

Public Functions

inline SparseBlockRow()¶

template<int DenseMatrixColumns> inline void dot(BlockVector<Scalar<ScalarType>, Dimension<DenseMatrixColumns>, VariableGroup<Variables...>> &v, Eigen::Matrix<ScalarType, RowDimension, DenseMatrixColumns> &result)¶

Block vector variant of the sparse dot product.

If the the block vector does not contain an element in the sparse block row, it is assumed to be 0. a.k.a. Row * v = result

Computes the dot product of this row with a dense column vector.

The variable container is used to determine the indices of each block. a.k.a. Row * v = result

template<typename VariableType> inline VariableColumns<VariableType> &getVariableMap()¶: Returns a std map of blocks for the given variable type.

inline void setZero()¶: Sets all current non zero blocks to zero.

Private Members

std::tuple<VariableColumns<Variables>...> columns¶