diff --git a/fuse_graphs/include/fuse_graphs/hash_graph.hpp b/fuse_graphs/include/fuse_graphs/hash_graph.hpp index 1f0daf527..6b33df7dd 100644 --- a/fuse_graphs/include/fuse_graphs/hash_graph.hpp +++ b/fuse_graphs/include/fuse_graphs/hash_graph.hpp @@ -59,7 +59,6 @@ #include #include - namespace fuse_graphs { @@ -86,14 +85,14 @@ class HashGraph : public fuse_core::Graph * * @param[in] params HashGraph parameters. */ - explicit HashGraph(const HashGraphParams & params = HashGraphParams()); + explicit HashGraph(const HashGraphParams& params = HashGraphParams()); /** * @brief Copy constructor * * Performs a deep copy of the graph */ - HashGraph(const HashGraph & other); + HashGraph(const HashGraph& other); /** * @brief Destructor @@ -105,7 +104,7 @@ class HashGraph : public fuse_core::Graph * * Performs a deep copy of the graph */ - HashGraph & operator=(const HashGraph & other); + HashGraph& operator=(const HashGraph& other); /** * @brief Clear all variables and constraints from the graph object. @@ -130,7 +129,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the constraint being searched for * @return True if this constraint already exists, False otherwise */ - bool constraintExists(const fuse_core::UUID & constraint_uuid) const noexcept override; + bool constraintExists(const fuse_core::UUID& constraint_uuid) const noexcept override; /** * @brief Add a new constraint to the graph @@ -160,7 +159,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the constraint to be removed * @return True if the constraint was removed, false otherwise */ - bool removeConstraint(const fuse_core::UUID & constraint_uuid) override; + bool removeConstraint(const fuse_core::UUID& constraint_uuid) override; /** * @brief Read-only access to a constraint from the graph by UUID @@ -172,8 +171,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the requested constraint * @return The constraint in the graph with the specified UUID */ - const fuse_core::Constraint & getConstraint(const fuse_core::UUID & constraint_uuid) const - override; + const fuse_core::Constraint& getConstraint(const fuse_core::UUID& constraint_uuid) const override; /** * @brief Read-only access to all of the constraints in the graph @@ -195,8 +193,7 @@ class HashGraph : public fuse_core::Graph * @return A read-only iterator range containing all constraints that involve the specified * variable */ - fuse_core::Graph::const_constraint_range getConnectedConstraints( - const fuse_core::UUID & variable_uuid) const override; + fuse_core::Graph::const_constraint_range getConnectedConstraints(const fuse_core::UUID& variable_uuid) const override; /** * @brief Check if the variable already exists in the graph @@ -207,7 +204,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the variable being searched for * @return True if this variable already exists, False otherwise */ - bool variableExists(const fuse_core::UUID & variable_uuid) const noexcept override; + bool variableExists(const fuse_core::UUID& variable_uuid) const noexcept override; /** * @brief Add a new variable to the graph @@ -235,7 +232,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the variable to be removed * @return True if the variable was removed, false otherwise */ - bool removeVariable(const fuse_core::UUID & variable_uuid) override; + bool removeVariable(const fuse_core::UUID& variable_uuid) override; /** * @brief Read-only access to a variable in the graph by UUID @@ -246,7 +243,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the requested variable * @return The variable in the graph with the specified UUID */ - const fuse_core::Variable & getVariable(const fuse_core::UUID & variable_uuid) const override; + const fuse_core::Variable& getVariable(const fuse_core::UUID& variable_uuid) const override; /** * @brief Read-only access to all of the variables in the graph @@ -275,7 +272,7 @@ class HashGraph : public fuse_core::Graph * optimization, or if the variable's value is allowed to change during * optimization. */ - void holdVariable(const fuse_core::UUID & variable_uuid, bool hold_constant = true) override; + void holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant = true) override; /** * @brief Check whether a variable is on hold or not @@ -283,7 +280,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The variable to test * @return True if the variable is on hold, false otherwise */ - bool isVariableOnHold(const fuse_core::UUID & variable_uuid) const override; + bool isVariableOnHold(const fuse_core::UUID& variable_uuid) const override; /** * @brief Compute the marginal covariance blocks for the requested set of variable pairs. @@ -310,11 +307,10 @@ class HashGraph : public fuse_core::Graph * variable's tangent space/local coordinates. Otherwise it is * computed in the variable's parameter space. */ - void getCovariance( - const std::vector> & covariance_requests, - std::vector> & covariance_matrices, - const ceres::Covariance::Options & options = ceres::Covariance::Options(), - const bool use_tangent_space = true) const override; + void getCovariance(const std::vector>& covariance_requests, + std::vector>& covariance_matrices, + const ceres::Covariance::Options& options = ceres::Covariance::Options(), + const bool use_tangent_space = true) const override; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -332,8 +328,7 @@ class HashGraph : public fuse_core::Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - ceres::Solver::Summary optimize(const ceres::Solver::Options & options = ceres::Solver::Options()) - override; + ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) override; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -350,10 +345,9 @@ class HashGraph : public fuse_core::Graph * @return 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; + 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; /** * @brief Evaluate the values of the current set of variables, given the current set of @@ -378,35 +372,29 @@ class HashGraph : public fuse_core::Graph * solver.googlesource.com/ceres-solver/+/master/include/ceres/problem.h#401 * @return True if the problem evaluation was successful; False, otherwise. */ - bool evaluate( - double * cost, std::vector * residuals = nullptr, - std::vector * gradient = nullptr, - const ceres::Problem::EvaluateOptions & options = ceres::Problem::EvaluateOptions()) const - override; + bool evaluate(double* cost, std::vector* residuals = nullptr, std::vector* gradient = nullptr, + const ceres::Problem::EvaluateOptions& options = ceres::Problem::EvaluateOptions()) const override; /** * @brief Print a human-readable description of the graph to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; protected: // Define some helpful typedefs - using Constraints = std::unordered_map; - using Variables = std::unordered_map; + using Constraints = std::unordered_map; + using Variables = std::unordered_map; using VariableSet = std::unordered_set; - using cross-reference = std::unordered_map, - fuse_core::uuid::hash>; + using CrossReference = std::unordered_map, fuse_core::uuid::hash>; - Constraints constraints_; //!< The set of all constraints - cross-reference 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 + 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 /** * @brief Populate a ceres::Problem object using the current set of variables and constraints @@ -416,7 +404,7 @@ class HashGraph : public fuse_core::Graph * * @param[out] problem The ceres::Problem object to modify */ - void createProblem(ceres::Problem & problem) const; + void createProblem(ceres::Problem& problem) const; private: // Allow Boost Serialization access to private methods @@ -429,15 +417,15 @@ class HashGraph : public fuse_core::Graph * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & constraints_; - archive & constraints_by_variable_uuid_; - archive & problem_options_; - archive & variables_; - archive & variables_on_hold_; + archive& boost::serialization::base_object(*this); + archive& constraints_; + archive& constraints_by_variable_uuid_; + archive& problem_options_; + archive& variables_; + archive& variables_on_hold_; } }; @@ -451,24 +439,22 @@ namespace serialization /** * @brief Serialize a ceres::Problem::Options object using Boost Serialization */ -template -void serialize( - Archive & archive, ceres::Problem::Options & options, - const unsigned int /* version */) +template +void serialize(Archive& archive, ceres::Problem::Options& options, const unsigned int /* version */) { - archive & options.cost_function_ownership; - archive & options.disable_all_safety_checks; - archive & options.enable_fast_removal; + archive& options.cost_function_ownership; + archive& options.disable_all_safety_checks; + archive& options.enable_fast_removal; #if !CERES_SUPPORTS_MANIFOLDS - archive & options.local_parameterization_ownership; + archive& options.local_parameterization_ownership; #else // Local parameterizations got marked as deprecated in favour of Manifold in version 2.1.0, see // https://github.com/ceres-solver/ceres-solver/commit/0141ca090c315db2f3c38e1731f0fe9754a4e4cc // and they got removed in 2.2.0, see // https://github.com/ceres-solver/ceres-solver/commit/68c53bb39552cd4abfd6381df08638285f7386b3 - archive & options.manifold_ownership; + archive& options.manifold_ownership; #endif - archive & options.loss_function_ownership; + archive& options.loss_function_ownership; } } // namespace serialization