Skip to content

Commit

Permalink
fix compilation
Browse files Browse the repository at this point in the history
  • Loading branch information
@henrygerardmoore
henrygerardmoore committed Sep 23, 2024
1 parent 181b59b commit 7a43bf7
Showing 1 changed file with 49 additions and 63 deletions.
112 changes: 49 additions & 63 deletions fuse_graphs/include/fuse_graphs/hash_graph.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,6 @@
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/unordered_set.hpp>


namespace fuse_graphs
{

Expand All @@ -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
Expand All @@ -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.
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -275,15 +272,15 @@ 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
*
* @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.
Expand All @@ -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<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;
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;

/**
* @brief Optimize the values of the current set of variables, given the current set of
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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<double> * residuals = nullptr,
std::vector<double> * gradient = nullptr,
const ceres::Problem::EvaluateOptions & options = ceres::Problem::EvaluateOptions()) const
override;
bool evaluate(double* cost, std::vector<double>* residuals = nullptr, std::vector<double>* 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<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 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 cross-reference = std::unordered_map<fuse_core::UUID, std::vector<fuse_core::UUID>,
fuse_core::uuid::hash>;
using CrossReference = std::unordered_map<fuse_core::UUID, std::vector<fuse_core::UUID>, 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
Expand All @@ -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
Expand All @@ -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<class Archive>
void serialize(Archive & archive, const unsigned int /* version */)
template <class Archive>
void serialize(Archive& archive, const unsigned int /* version */)
{
archive & boost::serialization::base_object<fuse_core::Graph>(*this);
archive & constraints_;
archive & constraints_by_variable_uuid_;
archive & problem_options_;
archive & variables_;
archive & variables_on_hold_;
archive& boost::serialization::base_object<fuse_core::Graph>(*this);
archive& constraints_;
archive& constraints_by_variable_uuid_;
archive& problem_options_;
archive& variables_;
archive& variables_on_hold_;
}
};

Expand All @@ -451,24 +439,22 @@ namespace serialization
/**
* @brief Serialize a ceres::Problem::Options object using Boost Serialization
*/
template<class Archive>
void serialize(
Archive & archive, ceres::Problem::Options & options,
const unsigned int /* version */)
template <class Archive>
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
Expand Down

0 comments on commit 7a43bf7

Please sign in to comment.