Class HashGraph

Inheritance Relationships

Base Type

  • public fuse_core::Graph

Class Documentation

class HashGraph : public fuse_core::Graph

This is a concrete implementation of the Graph interface using hashmaps to store the constraints and variables.

This is reasonable graph implementation when a large number of variables and constraints are expected, such as with full SLAM and mapping applications. The hashmap overhead may be too expensive for use in high-frequency systems with a basically fixed graph size. Something base on a boost::flat_map or similar may perform better in those situations. The final decision on the graph type should be based actual performance testing.

This class is not thread-safe. If used in a multi-threaded application, standard thread synchronization techniques should be used to guard access to the graph.

Public Functions

explicit HashGraph(const HashGraphParams &params = HashGraphParams())

Constructor.

Parameters:

params – [in] HashGraph parameters.

HashGraph(const HashGraph &other)

Copy constructor.

Performs a deep copy of the graph

virtual ~HashGraph() = default

Destructor.

HashGraph &operator=(const HashGraph &other)

Assignment operator.

Performs a deep copy of the graph

void clear() override

Clear all variables and constraints from the graph object.

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

fuse_core::Graph::UniquePtr clone() const override

Return a deep copy of the graph object.

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

bool constraintExists(const fuse_core::UUID &constraint_uuid) const noexcept override

Check if the constraint already exists in the graph.

Exceptions: None Complexity: O(1) (average)

Parameters:

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

Returns:

True if this constraint already exists, False otherwise

bool addConstraint(fuse_core::Constraint::SharedPtr constraint) override

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.

Behavior: If this constraint already exists in the graph, the function will return false. Exceptions: If the constraint’s variables do not exist in the graph, a std::logic_error exception will be thrown. If any unexpected errors occur, an exception will be thrown. Complexity: O(1) (average)

Parameters:

constraint – [in] The new constraint to be added

Returns:

True if the constraint was added, false otherwise

bool removeConstraint(const fuse_core::UUID &constraint_uuid) override

Remove a constraint from the graph.

Behavior: If this constraint does not exist in the graph, the function will return false. Exceptions: If the constraint UUID does not exist, a std::out_of_range exception will be thrown. If any unexpected errors occur, an exception will be thrown. Complexity: O(1) (average)

Parameters:

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

Returns:

True if the constraint was removed, false otherwise

const fuse_core::Constraint &getConstraint(const fuse_core::UUID &constraint_uuid) const override

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

Exceptions: If the constraint UUID does not exist, a std::out_of_range exception will be thrown. Complexity: O(1) (average)

Parameters:

constraint_uuid – [in] The UUID of the requested constraint

Returns:

The constraint in the graph with the specified UUID

fuse_core::Graph::const_constraint_range getConstraints() const noexcept override

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

Behavior: This function returns iterators pointing to the beginning and end of the collection. No copies of the constraints are performed at this time. Exceptions: None Complexity: O(1) This function returns in constant time. However, iterating through the returned range is naturally O(N).

Returns:

A read-only iterator range containing all constraints

fuse_core::Graph::const_constraint_range getConnectedConstraints(const fuse_core::UUID &variable_uuid) const override

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

bool variableExists(const fuse_core::UUID &variable_uuid) const noexcept override

Check if the variable already exists in the graph.

Exceptions: None Complexity: O(1) (average)

Parameters:

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

Returns:

True if this variable already exists, False otherwise

bool addVariable(fuse_core::Variable::SharedPtr variable) override

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.

Behavior: If this variable already exists in the graph, the function will return false. Exceptions: If any unexpected errors occur, an exception will be thrown. Complexity: O(1) (average)

Parameters:

variable – [in] The new variable to be added

Returns:

True if the variable was added, false otherwise

bool removeVariable(const fuse_core::UUID &variable_uuid) override

Remove a variable from the graph.

Exceptions: If constraints still exist that refer to this variable, a std::logic_error exception will be thrown. If an unexpected error occurs during the removal, an exception will be thrown. Complexity: O(1) (average)

Parameters:

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

Returns:

True if the variable was removed, false otherwise

const fuse_core::Variable &getVariable(const fuse_core::UUID &variable_uuid) const override

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

Exceptions: If the variable UUID does not exist, a std::out_of_range exception will be thrown. Complexity: O(1) (average)

Parameters:

variable_uuid – [in] The UUID of the requested variable

Returns:

The variable in the graph with the specified UUID

fuse_core::Graph::const_variable_range getVariables() const noexcept override

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

Behavior: This function returns iterators pointing to the beginning and end of the collection. No copies of the variables are performed at this time. Exceptions: None Complexity: O(1) This function returns in constant time. However, iterating through the returned range is naturally O(N).

Returns:

A read-only iterator range containing all variables

void holdVariable(const fuse_core::UUID &variable_uuid, bool hold_constant = true) override

Configure a variable to hold its current value 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.

Exceptions: If the variable does not exist, a std::out_of_range exception will be thrown. Complexity: O(1) (average)

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.

bool isVariableOnHold(const fuse_core::UUID &variable_uuid) const override

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

void getCovariance(const std::vector<std::pair<fuse_core::UUID, fuse_core::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 override

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.

Exceptions: If the request contains unknown variables, a std::out_of_range exception will be thrown. If the covariance calculation fails, a std::runtime_error exception will be thrown. Complexity: O(N) in the best case, O(N^3) in the worst case, where N is the total number of variables in the graph. In practice, it is significantly cheaper than the worst-case bound, but it is still an expensive operation.

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.

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

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. This is where the “work” of the optimization system is performed. As such, it is often a long-running process.

Complexity: O(N) in the best case, O(M*N^3) in the worst case, where N is the total number of variables in the graph, and M is the maximum number of allowed iterations.

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

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)) override

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

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

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.

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

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

Parameters:

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

Protected Types

using Constraints = std::unordered_map<fuse_core::UUID, fuse_core::Constraint::SharedPtr, fuse_core::uuid::hash>
using Variables = std::unordered_map<fuse_core::UUID, fuse_core::Variable::SharedPtr, fuse_core::uuid::hash>
using VariableSet = std::unordered_set<fuse_core::UUID, fuse_core::uuid::hash>
using CrossReference = std::unordered_map<fuse_core::UUID, std::vector<fuse_core::UUID>, fuse_core::uuid::hash>

Protected Functions

void createProblem(ceres::Problem &problem) const

Populate a ceres::Problem object using the current set of variables and constraints.

This function assumes the provided variables and constraints are consistent. No checks are performed for missing variables or constraints.

Parameters:

problem – [out] The ceres::Problem object to modify

Protected Attributes

Constraints constraints_

The set of all constraints.

CrossReference constraints_by_variable_uuid_

Index all of the constraints by variable uuids.

ceres::Problem::Options problem_options_

User-defined options to be applied to all constructed ceres::Problems

Variables variables_

The set of all variables.

VariableSet variables_on_hold_

The set of variables that should be held constant.

Friends

friend class boost::serialization::access