Class Graph

• Defined in File graph.hpp

Nested Relationships

Nested Types

• Struct Graph::ConstraintCost

Class Documentation

class Graph

This is an interface definition describing the collection of constraints and variables that form the factor graph, a graphical model of a nonlinear least-squares problem.

Methods are provided to add, remove, and access the constraints and variables by several criteria, as well as to optimize the variable values. Derived classes may store the constraints and variables using any mechanism, but the same interface must be provided.

Public Types

using const_constraint_range = boost::any_range<const Constraint, boost::forward_traversal_tag>

A range of fuse_core::Constraint objects.

An object representing a range defined by two iterators. It has begin() and end() methods (which means it can be used in range-based for loops), an empty() method, and a front() method for directly accessing the first member. When dereferenced, an iterator returns a const Constraint&.

using const_variable_range = boost::any_range<const Variable, boost::forward_traversal_tag>

A range of fuse_core::Variable objects.

An object representing a range defined by two iterators. It has begin() and end() methods (which means it can be used in range-based for loops), an empty() method, and a front() method for directly accessing the first member. When dereferenced, an iterator returns a const Variable&.

Public Functions

Graph() = default

Constructor.

virtual ~Graph() = default

Destructor.

virtual std::string type() const = 0

Returns a unique name for this graph 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.

virtual void clear() = 0

Clear all variables and constraints from the graph object.

The object should be equivalent to a newly constructed object after clear() has been called.

virtual Graph::UniquePtr clone() const = 0

Return a deep copy of the graph object.

This should include deep copies of all variables and constraints; not pointer copies.

virtual bool constraintExists(const UUID &constraint_uuid) const = 0

Check if the constraint already exists in the graph.

Parameters:

constraint_uuid – [in] The UUID of the constraint being searched for

Returns:

True if this constraint already exists, False otherwise

virtual bool addConstraint(Constraint::SharedPtr constraint) = 0

Add a new constraint to the graph.

Any referenced variables must exist in the graph before the constraint is added. The Graph will share ownership of the constraint. If this constraint already exists in the graph, the function will return false.

Parameters:

constraint – [in] The new constraint to be added

Returns:

True if the constraint was added, false otherwise

virtual bool removeConstraint(const UUID &constraint_uuid) = 0

Remove a constraint from the graph.

Parameters:

constraint_uuid – [in] The UUID of the constraint to be removed

Returns:

True if the constraint was removed, false otherwise

virtual const Constraint &getConstraint(const UUID &constraint_uuid) const = 0

Read-only access to a constraint from the graph by UUID.

If the requested constraint does not exist, an exception will be thrown.

Parameters:

constraint_uuid – [in] The UUID of the requested constraint

Returns:

The constraint in the graph with the specified UUID

virtual const_constraint_range getConstraints() const = 0

Read-only access to all of the constraints in the graph.

Returns:

A read-only iterator range containing all constraints

virtual const_constraint_range getConnectedConstraints(const UUID &variable_uuid) const = 0

Read-only access to the subset of constraints that are connected to the specified variable.

Parameters:

variable_uuid – [in] The UUID of the variable of interest

Returns:

A read-only iterator range containing all constraints that involve the specified variable

virtual bool variableExists(const UUID &variable_uuid) const = 0

Check if the variable already exists in the graph.

Parameters:

variable_uuid – [in] The UUID of the variable being searched for

Returns:

True if this variable already exists, False otherwise

virtual bool addVariable(Variable::SharedPtr variable) = 0

Add a new variable to the graph.

The Graph will share ownership of the Variable. If this variable already exists in the graph, the function will return false.

Parameters:

variable – [in] The new variable to be added

Returns:

True if the variable was added, false otherwise

virtual bool removeVariable(const UUID &variable_uuid) = 0

Remove a variable from the graph.

Parameters:

variable_uuid – [in] The UUID of the variable to be removed

Returns:

True if the variable was removed, false otherwise

virtual const Variable &getVariable(const UUID &variable_uuid) const = 0

Read-only access to a variable in the graph by UUID.

If the requested variable does not exist, an empty pointer will be returned.

Parameters:

variable_uuid – [in] The UUID of the requested variable

Returns:

The variable in the graph with the specified UUID

virtual const_variable_range getVariables() const = 0

Read-only access to all of the variables in the graph.

Returns:

A read-only iterator range containing all variables

virtual const_variable_range getConnectedVariables(const UUID &constraint_uuid) const

Read-only access to the subset of variables that are connected to the specified constraint.

Parameters:

constraint_uuid – [in] The UUID of the constraint of interest

Returns:

A read-only iterator range containing all variables that involve the specified constraint

virtual void holdVariable(const UUID &variable_uuid, bool hold_constant = true) = 0

Configure a variable to hold its current value constant during optimization.

Once set, the specified variable’s value will no longer change during any subsequent optimization. To ‘unhold’ a previously held variable, call Graph::holdVariable() with the hold_constant parameter set to false.

Parameters:

• variable_uuid – [in] The variable to adjust

• hold_constant – [in] Flag indicating if the variable’s value should be held constant during optimization, or if the variable’s value is allowed to change during optimization.

virtual bool isVariableOnHold(const UUID &variable_uuid) const = 0

Check whether a variable is on hold or not.

Parameters:

variable_uuid – [in] The variable to test

Returns:

True if the variable is on hold, false otherwise

virtual void getCovariance(const std::vector<std::pair<UUID, UUID>> &covariance_requests, std::vector<std::vector<double>> &covariance_matrices, const ceres::Covariance::Options &options = ceres::Covariance::Options(), const bool use_tangent_space = true) const = 0

Compute the marginal covariance blocks for the requested set of variable pairs.

To compute the marginal variance of a single variable, simply supply the same variable UUID for both members of of the request pair. Computing the marginal covariance is an expensive operation; grouping multiple variable pairs into a single call will be much faster than calling this function for each pair individually. The marginal covariances can only be computed after calling Graph::computeUpdates() or Graph::optimize().

Parameters:

• covariance_requests – [in] A set of variable UUID pairs for which the marginal covariance is desired.

• covariance_matrices – [out] The dense covariance blocks of the requests.

• options – [in] A Ceres Covariance Options structure that controls the method and settings used to compute the covariance blocks.

• use_tangent_space – [in] Flag indicating if the covariance should be computed in the variable’s tangent space/local coordinates. Otherwise it is computed in the variable’s parameter space.

void update(const Transaction &transaction)

Update the graph with the contents of a transaction.

Parameters:

transaction – [in] A set of variable and constraints additions and deletions

virtual ceres::Solver::Summary optimize(const ceres::Solver::Options &options = ceres::Solver::Options()) = 0

Optimize the values of the current set of variables, given the current set of constraints.

After the call, the values in the graph will be updated to the latest values.

Parameters:

options – [in] An optional Ceres Solver::Options object that controls various aspects of the optimizer. See https://ceres-solver.googlesource.com/ceres- solver/+/master/include/ceres/solver.h#59

Returns:

A Ceres Solver Summary structure containing information about the optimization process

virtual ceres::Solver::Summary optimizeFor(const rclcpp::Duration &max_optimization_time, const ceres::Solver::Options &options = ceres::Solver::Options(), rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) = 0

Optimize the values of the current set of variables, given the current set of constraints for a maximum amount of time.

The max_optimization_time should be viewed as a “best effort” limit, and the actual optimization time may exceed this limit by a small amount. After the call, the values in the graph will be updated to the latest values.

Parameters:

• max_optimization_time – [in] The maximum allowed duration of the optimization call

• options – [in] An optional Ceres Solver::Options object that controls various aspects of the optimizer. See https://ceres-solver.googlesource.com/ceres- solver/+/master/include/ceres/solver.h#59

Returns:

A Ceres Solver Summary structure containing information about the optimization process

virtual bool evaluate(double *cost, std::vector<double> *residuals = nullptr, std::vector<double> *gradient = nullptr, const ceres::Problem::EvaluateOptions &options = ceres::Problem::EvaluateOptions()) const = 0

Evalute the values of the current set of variables, given the current set of constraints.

The values in the graph do not change after the call.

If any of the output arguments is nullptr, it will not be evaluated. This mimics the ceres::Problem::Evaluate method API. Here all output arguments default to nullptr except for the cost.

TODO(efernandez) support jacobian output argument The jacobian output argument is not exposed at the moment because its type is a CRSMatrix, that probably needs to be converted to another type.

Parameters:

• cost – [out] The cost of the entire problem represented by the graph.

• residuals – [out] The residuals of all constraints.

• gradient – [out] The gradient for all constraints evaluated at the values of the current set of variables.

• options – [in] An optional Ceres Problem::EvaluateOptions object that controls various aspects of the problem evaluation. See https://ceres- solver.googlesource.com/ceres-solver/+/master/include/ceres/problem.h#401

Returns:

True if the problem evaluation was successful; False, otherwise.

template<class UuidForwardIterator, class OutputIterator>
void getConstraintCosts(UuidForwardIterator first, UuidForwardIterator last, OutputIterator output)

Compute the residual information for a collection of constraints.

If any of the requested constraints does not exist, an exception will be thrown.

Parameters:

• first – [in] An iterator pointing to the first UUID of the desired constraints

• last – [in] An iterator pointing to one passed the last UUID of the desired constraints

• output – [out] An output iterator capable of assignment to a ConstraintCost object

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

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

Parameters:

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

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

Serialize this graph 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 graph into

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

Serialize this graph 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 graph into

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

Deserialize data from the provided binary archive into this graph.

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

archive >> *this;

Parameters:

archive – [in] - The archive holding serialized graph data

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

Deserialize data from the provided text archive into this graph.

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

archive >> *this;

Parameters:

archive – [in] - The archive holding serialized graph data

Friends

friend class boost::serialization::access

struct ConstraintCost

Structure containing the cost and residual information for a single constraint.

Public Members

double cost = {}

The pre-loss-function cost of the constraint, computed as the norm of the residuals

double loss = {}

The final cost of the constraint after any loss functions have been applied

std::vector<double> residuals

The individual residuals for the constraint.