Template Class RelativeConstraint

Inheritance Relationships

Base Type

  • public fuse_core::Constraint

Class Documentation

template<class Variable>
class RelativeConstraint : public fuse_core::Constraint

A constraint that represents a measurement on the difference between two variables.

This type of constraint arises in many situations. Some sensors, such as wheel encoders or inertial strap-down systems, measure a change in the variable state instead of measuring the state directly. This constraint holds the measured change between two variables and the measurement uncertainty/covariance.

The difference between the two variables is constructed element-wise. If element-wise subtraction is not the correct operation for a specific variable type (e.g. 3D rotations), a custom constraint or template specialization will be needed.

Public Functions

RelativeConstraint() = default

Default constructor.

RelativeConstraint(const std::string &source, const Variable &variable1, const Variable &variable2, const fuse_core::VectorXd &delta, const fuse_core::MatrixXd &covariance)

Create a constraint on the change of all dimensions between the two target variables.

Parameters:

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

  • variable1 – [in] The first variable

  • variable2 – [in] The second variable

  • delta – [in] The measured change between variable1 and variable2

  • covariance – [in] The measurement uncertainty

RelativeConstraint(const std::string &source, const Variable &variable1, const Variable &variable2, const fuse_core::VectorXd &delta, const fuse_core::MatrixXd &covariance, const std::vector<size_t> &indices)

Constructor.

Create a constraint on the change of a partial set of dimensions between the two target variables

Parameters:

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

  • variable1 – [in] The first variable

  • variable2 – [in] The second variable

  • partial_delta – [in] The measured change of the subset of dimensions in the order defined by indices

  • partial_covariance – [in] The uncertainty of the change of the subset of dimensions in the order defined by indices.

  • indices – [in] The set of indices corresponding to the measured dimensions

virtual ~RelativeConstraint() = default

Destructor.

inline const fuse_core::VectorXd &delta() const

Read-only access to the measured change between variables.

All dimensions are present, even if only a partial set of dimensions were measured. Dimensions are in the order defined by the variable, not the order defined by the indices parameter. All unmeasured variable dimensions are set to zero.

inline const fuse_core::MatrixXd &sqrtInformation() const

Read-only access to the square root information matrix.

Dimensions are in the order defined by the variable, not the order defined by the indices parameter. The square root information matrix will have size measured_dimensions X variable_dimensions. If only a partial set of dimensions are measured, then this matrix will not be square.

fuse_core::MatrixXd covariance() const

Compute the measurement covariance matrix.

Dimensions are in the order defined by the variable, not the order defined by the indices parameter. The covariance matrix will always be square, with size variable_dimensions X variable_dimensions. If only a subset of dimensions are measured, then some rows/columns will be zero. This will result in a rank-deficient covariance matrix. You have been warned.

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 takes ownership of the pointer and delete it during destruction.

Returns:

A base pointer to an instance of a derived CostFunction.

inline ceres::CostFunction *costFunction() const

Protected Attributes

fuse_core::VectorXd delta_

The measured change between the two variables.

fuse_core::MatrixXd sqrt_information_

The square root information matrix.

Friends

friend class boost::serialization::access