Struct sym::MarginalizationFactor

template<typename ScalarType>
struct MarginalizationFactor

Marginalization factors are linear approximations of information we remove from the optimization problem to bound its size. The struct contains all the data needed to create a marginalization factor.

We are storing the factor in a general quadratic form which will be a term used in a non-linear least squares optimization. Taking inspiration from the LevenbergMarquardtSolver doc string: e(x) ~= 0.5 * dx.T * J.T * J * dx + b.T * J * dx + 0.5 * b.T * b where J is the Jacobian of the residual function f(x) and b = f(x0) is the residual at the linearization point.

We store H = J.T * J (ie. the Gauss-Newton approximation of the Hessian), rhs = J.T * b, c = b.T * b (dropping the 0.5 factor by convention). This simplifies to: e(x) ~= 0.5 * dx.T * H * dx + rhs.T * dx + 0.5 * c We end up solving the larger system in the form of H’ * x = b’ to find the optimal value of x.

Public Types

using Scalar = ScalarType

using LcmType = internal::MarginalizationFactorLcmTypeT<Scalar>

Public Functions

LcmType GetLcmType() const

Public Members

MatrixX<Scalar> H = {}

VectorX<Scalar> rhs = {}

Scalar c = {}

Values<Scalar> linearization_values = {}

std::vector<Key> keys

Public Static Functions

static MarginalizationFactor FromLcmType(const LcmType &msg)