Class Constraint

Class Documentation

class Constraint

The Constraint interface definition.

A Constraint defines a cost function that is connected to one or more variables. This base class defines the required interface of all Constraint objects, and holds the ordered list of involved variable UUIDs. All other functionality is left to the derived classes to implement.

Most importantly, the implementation of the cost function is left to the derived classes, allowing arbitrarily complex sensor models to be implemented outside of the core fuse packages. The cost function must be a valid ceres::CostFunction object. Ceres provides many nice features to make implementing the cost function easier, including an automatic differentiation system. Please see the Ceres documentation for details on creating valid ceres::CostFunction objects (http://ceres-solver.org/nnls_modeling.html). In addition to the cost function itself, an optional loss function may be provided. Loss functions provide a mechanism for reducing the impact of outlier measurements on the final optimization results. Again, see the Ceres documentation for details (http://ceres- solver.org/nnls_modeling.html::lossfunction).

Public Functions

Constraint() = default

Default constructor.

Constraint(const std::string &source, std::initializer_list<UUID> variable_uuid_list)

Constructor.

Accepts an arbitrary number of variable UUIDs directly. It can be called like:

Constraint("source", {uuid1, uuid2, uuid3});

Parameters:

variable_uuid_list[in] The list of involved variable UUIDs

template<typename VariableUuidIterator>
Constraint(const std::string &source, VariableUuidIterator first, VariableUuidIterator last)

Constructor.

Accepts an arbitrary number of variable UUIDs stored in a container using iterators.

virtual ~Constraint() = default

Destructor.

virtual std::string type() const = 0

Returns a unique name for this constraint type.

The constraint type string must be unique for each class. As such, the fully-qualified class name is an excellent choice for the type string.

inline const UUID &uuid() const

Returns the UUID for this constraint.

Each constraint will generate a unique, random UUID during construction.

inline const std::string &source() const

Returns the name of the sensor or motion model that generated this constraint.

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

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

Parameters:

stream – The stream to write to. Defaults to stdout.

virtual ceres::CostFunction *costFunction() const = 0

Create a new Ceres cost function and return a raw pointer to it.

The Ceres interface requires a raw pointer. Ceres will take ownership of the pointer and promises to properly delete the cost function when it is done. Additionally, fuse promises that the Constraint object will outlive any generated cost functions (i.e. the Ceres objects will be destroyed before the Constraint objects). This guarantee may allow optimizations for the creation of the cost function objects.

Returns:

A base pointer to an instance of a derived ceres::CostFunction.

inline Loss::SharedPtr loss() const

Read-only access to the loss.

The loss interfaces wraps a ceres::LossFunction that can be accessed directly with lossFunction().

Returns:

A base shared pointer to an instance of a derived Loss.

inline void loss(Loss::SharedPtr loss)

Set the constraint loss function.

Parameters:

loss[in] - The loss function

inline ceres::LossFunction *lossFunction() const

Read-only access to the Ceres loss function.

Returns:

A base pointer to an instance of a derived ceres::LossFunction.

virtual Constraint::UniquePtr clone() const = 0

Perform a deep copy of the Constraint and return a unique pointer to the copy.

This can/should be implemented as follows in all derived classes:

return Derived::make_unique(*this);

Returns:

A unique pointer to a new instance of the most-derived Constraint

inline const std::vector<UUID> &variables() const

Read-only access to the ordered list of variable UUIDs involved in this constraint.

virtual void serialize(fuse_core::BinaryOutputArchive&) const = 0

Serialize this Constraint into the provided binary archive.

This can/should be implemented as follows in all derived classes:

archive << *this;

Parameters:

archive[out] - The archive to serialize this constraint into

virtual void serialize(fuse_core::TextOutputArchive&) const = 0

Serialize this Constraint into the provided text archive.

This can/should be implemented as follows in all derived classes:

archive << *this;

Parameters:

archive[out] - The archive to serialize this constraint into

virtual void deserialize(fuse_core::BinaryInputArchive&) = 0

Deserialize data from the provided binary archive into this Constraint.

This can/should be implemented as follows in all derived classes:

archive >> *this;

Parameters:

archive[in] - The archive holding serialized Constraint data

virtual void deserialize(fuse_core::TextInputArchive&) = 0

Deserialize data from the provided text archive into this Constraint.

This can/should be implemented as follows in all derived classes:

archive >> *this;

Parameters:

archive[in] - The archive holding serialized Constraint data

Friends

friend class boost::serialization::access