Class MarginalConstraint

Inheritance Relationships

Base Type

  • public fuse_core::Constraint

Class Documentation

class MarginalConstraint : public fuse_core::Constraint

A constraint that represents remaining marginal information on a set of variables.

The marginal constraint cost function is of the form: cost = A1 * (x1 - x1_bar) + A2 * (x2 - x2_bar) + … + b where x_bar is the linearization point of the variable taken from the variable value at the time of construction, and the minus operator in implemented in the variable’s local parameterization.

Public Functions

MarginalConstraint() = default

Default constructor.

template<typename VariableIterator, typename MatrixIterator>
MarginalConstraint(const std::string &source, VariableIterator first_variable, VariableIterator last_variable, MatrixIterator first_A, MatrixIterator last_A, const fuse_core::VectorXd &b)

Create a linear/marginal constraint.

The variable iterators and matrix iterators must be the same size. Further, all A matrices and the b vector must have the same number of rows, and the number of columns of each A matrix must match the localSize() of its associated variable.

Parameters:

  • source – [in] The name of the sensor or motion model that generated this constraint

  • first_variable – [in] Iterator pointing to the first involved variable for this constraint

  • last_variable – [in] Iterator pointing to one past the last involved variable for this constraint

  • first_A – [in] Iterator pointing to the first A matrix, associated with the first variable

  • last_A – [in] Iterator pointing to one past the last A matrix

  • b – [in] The b vector of the marginal cost (of the form A*(x - x_bar) + b)

virtual ~MarginalConstraint() = default

Destructor.

inline const std::vector<fuse_core::MatrixXd> &A() const

Read-only access to the A matrices of the marginal constraint.

inline const fuse_core::VectorXd &b() const

Read-only access to the b vector of the marginal constraint.

inline const std::vector<fuse_core::VectorXd> &x_bar() const

Read-only access to the variable linearization points, x_bar.

inline const std::vector<fuse_core::LocalParameterization::SharedPtr> &localParameterizations() const

Read-only access to the variable local parameterizations.

void print(std::ostream &stream = std::cout) const override

Print a human-readable description of the constraint to the provided stream.

Parameters:

stream – [out] The stream to write to. Defaults to stdout.

ceres::CostFunction *costFunction() const override

Construct an instance of this constraint’s cost function.

The function caller will own the new cost function instance. It is the responsibility of the caller to delete the cost function object when it is no longer needed. If the pointer is provided to a Ceres::Problem object, the Ceres::Problem object will take ownership of the pointer and delete it during destruction.

Returns:

A base pointer to an instance of a derived CostFunction.

Protected Attributes

std::vector<fuse_core::MatrixXd> A_

The A matrices of the marginal constraint.

fuse_core::VectorXd b_

The b vector of the marginal constraint.

The local parameterizations

std::vector<fuse_core::LocalParameterization::SharedPtr> local_parameterizations_
std::vector<fuse_core::VectorXd> x_bar_

The linearization point of each involved variable.

Friends

friend class boost::serialization::access