Function fuse_constraints::detail::linearize

Defined in File marginalize_variables.hpp

Function Documentation

LinearTerm fuse_constraints::detail::linearize(const fuse_core::Constraint &constraint, const fuse_core::Graph &graph, const UuidOrdering &elimination_order)

Linearize the nonlinear constraint.

Variable UUIDs are converted into indices using the elimination_order. The variable linearization points are extracted from the current variable values in the graph.

Parameters:

constraint – [in] The constraint to linearize
graph – [in] A graph containing, at least, the variables involved in the constraint
elimination_order – [in] A mapping from variable UUID to elimination order

Returns:

A LinearTerm consisting of Jacobian blocks associated with each involved variable in elimination order