Skip to content

Commit

Permalink
Port support for Ceres 2.1.0 Manifold class into ROS 2 Rolling (locus…
Browse files Browse the repository at this point in the history 
…robotics#366)

* Support gcc12 and ceres 2.1.0
* Add support for the Manifold class when using Ceres Solver version 2.1.0 and above
* General clean up for Ceres 2.2.0 support
* Updated serialization support to be backwards compatible with previously serialized files
* Formatting changes required for ROS 2 Rolling / Ubuntu Noble
  • Loading branch information
@svwilliams @giafranchini
svwilliams authored and giafranchini committed Nov 2, 2024
1 parent ac96de6 commit fe121e1
Show file tree
Hide file tree
Showing 19 changed files with 330 additions and 330 deletions.
2 changes: 1 addition & 1 deletion fuse_constraints/src/marginalize_variables.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,7 +192,7 @@ fuse_core::Transaction marginalizeVariables(
[&elimination_order, &marginalized_variables](const fuse_core::UUID & variable_uuid)
{
return elimination_order.exists(variable_uuid) &&
elimination_order.at(variable_uuid) < marginalized_variables.size();
elimination_order.at(variable_uuid) < marginalized_variables.size();
})); // NOLINT

fuse_core::Transaction transaction;
Expand Down
6 changes: 3 additions & 3 deletions fuse_constraints/src/variable_constraints.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,9 +52,9 @@ bool VariableConstraints::empty() const
size_t VariableConstraints::size() const
{
auto sum_edges = [](const size_t input, const ConstraintCollection & edges)
{
return input + edges.size();
};
{
return input + edges.size();
};
return std::accumulate(variable_constraints_.begin(), variable_constraints_.end(), 0u, sum_edges);
}

Expand Down
2 changes: 1 addition & 1 deletion fuse_core/include/fuse_core/callback_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ template<typename T>
class CallbackWrapper : public CallbackWrapperBase
{
public:
using CallbackFunction = std::function<T(void)>;
using CallbackFunction = std::function<T (void)>;

/**
* @brief Constructor
Expand Down
28 changes: 14 additions & 14 deletions fuse_core/include/fuse_core/ceres_options.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,30 +54,30 @@
* For a given Ceres Solver Option <T>, the function ToString calls ceres::<T>ToString
*/
#define CERES_OPTION_TO_STRING_DEFINITION(Option) \
static inline const char * ToString(ceres::Option value) \
{ \
return ceres::Option ## ToString(value); \
}
static inline const char * ToString(ceres::Option value) \
{ \
return ceres::Option ## ToString(value); \
}

/**
* Defines FromString overloaded function to Ceres Options.
*
* For a given Ceres Solver Option <T>, the function FromString calls ceres::StringTo<T>
*/
#define CERES_OPTION_FROM_STRING_DEFINITION(Option) \
static inline bool FromString(std::string string_value, ceres::Option * value) \
{ \
return ceres::StringTo ## Option(string_value, value); \
}
static inline bool FromString(std::string string_value, ceres::Option * value) \
{ \
return ceres::StringTo ## Option(string_value, value); \
}

/**
* Defines ToString and FromString overloaded functions for Ceres Options.
*
* See CERES_OPTION_TO_STRING_DEFINITION and CERES_OPTION_FROM_STRING_DEFINITION.
*/
#define CERES_OPTION_STRING_DEFINITIONS(Option) \
CERES_OPTION_TO_STRING_DEFINITION(Option) \
CERES_OPTION_FROM_STRING_DEFINITION(Option)
CERES_OPTION_TO_STRING_DEFINITION(Option) \
CERES_OPTION_FROM_STRING_DEFINITION(Option)

#if !CERES_VERSION_AT_LEAST(2, 0, 0)
/**
Expand All @@ -99,8 +99,8 @@ static void UpperCase(std::string * input)
inline const char * LoggingTypeToString(LoggingType type)
{
switch (type) {
CASESTR(SILENT);
CASESTR(PER_MINIMIZER_ITERATION);
CASESTR(SILENT);
CASESTR(PER_MINIMIZER_ITERATION);
default:
return "UNKNOWN";
}
Expand All @@ -117,8 +117,8 @@ inline bool StringToLoggingType(std::string value, LoggingType * type)
inline const char * DumpFormatTypeToString(DumpFormatType type)
{
switch (type) {
CASESTR(CONSOLE);
CASESTR(TEXTFILE);
CASESTR(CONSOLE);
CASESTR(TEXTFILE);
default:
return "UNKNOWN";
}
Expand Down
78 changes: 39 additions & 39 deletions fuse_core/include/fuse_core/constraint.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,10 +67,10 @@
* @endcode
*/
#define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \
fuse_core::Constraint::UniquePtr clone() const override \
{ \
return __VA_ARGS__::make_unique(*this); \
}
fuse_core::Constraint::UniquePtr clone() const override \
{ \
return __VA_ARGS__::make_unique(*this); \
}

/**
* @brief Implementation of the serialize() and deserialize() member functions for derived classes
Expand All @@ -86,22 +86,22 @@
* @endcode
*/
#define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \
void serialize(fuse_core::BinaryOutputArchive & archive) const override \
{ \
archive << *this; \
} /* NOLINT */ \
void serialize(fuse_core::TextOutputArchive & archive) const override \
{ \
archive << *this; \
} /* NOLINT */ \
void deserialize(fuse_core::BinaryInputArchive & archive) override \
{ \
archive >> *this; \
} /* NOLINT */ \
void deserialize(fuse_core::TextInputArchive & archive) override \
{ \
archive >> *this; \
}
void serialize(fuse_core::BinaryOutputArchive & archive) const override \
{ \
archive << *this; \
} /* NOLINT */ \
void serialize(fuse_core::TextOutputArchive & archive) const override \
{ \
archive << *this; \
} /* NOLINT */ \
void deserialize(fuse_core::BinaryInputArchive & archive) override \
{ \
archive >> *this; \
} /* NOLINT */ \
void deserialize(fuse_core::TextInputArchive & archive) override \
{ \
archive >> *this; \
}

/**
* @brief Implements the type() member function using the suggested implementation
Expand All @@ -119,17 +119,17 @@
* @endcode
*/
#define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \
struct detail \
{ \
static std::string type() \
{ \
return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
} /* NOLINT */ \
}; /* NOLINT */ \
std::string type() const override \
{ \
return detail::type(); \
}
struct detail \
{ \
static std::string type() \
{ \
return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
} /* NOLINT */ \
}; /* NOLINT */ \
std::string type() const override \
{ \
return detail::type(); \
}

/**
* @brief Convenience function that creates the required pointer aliases, clone() method, and type()
Expand All @@ -146,10 +146,10 @@
* @endcode
*/
#define FUSE_CONSTRAINT_DEFINITIONS(...) \
FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)
FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)

/**
* @brief Convenience function that creates the required pointer aliases, clone() method, and type()
Expand All @@ -166,10 +166,10 @@
* @endcode
*/
#define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \
FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \
FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)
FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \
FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)


namespace fuse_core
Expand Down
34 changes: 17 additions & 17 deletions fuse_core/include/fuse_core/eigen_gtest.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,23 +108,23 @@ AssertionResult AssertMatrixNearHelper(
// Internal macro for implementing {EXPECT|ASSERT}_MATRIX_EQ.
// Don't use this in your code.
#define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \
GTEST_ASSERT_( \
::testing::AssertMatrixEqualHelper( \
#v1, \
#v2, \
v1, \
v2), on_failure)
GTEST_ASSERT_( \
::testing::AssertMatrixEqualHelper( \
#v1, \
#v2, \
v1, \
v2), on_failure)

// Internal macro for implementing {EXPECT|ASSERT}_MATRIX_NEAR.
// Don't use this in your code.
#define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \
GTEST_ASSERT_( \
::testing::AssertMatrixNearHelper( \
#v1, \
#v2, \
v1, \
v2, \
tol), on_failure)
GTEST_ASSERT_( \
::testing::AssertMatrixNearHelper( \
#v1, \
#v2, \
v1, \
v2, \
tol), on_failure)

// Define gtest macros for use with Eigen

Expand All @@ -137,7 +137,7 @@ AssertionResult AssertMatrixNearHelper(
* @param[in] v2 The actual matrix
*/
#define EXPECT_MATRIX_EQ(v1, v2) \
GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_)
GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_)

/**
* @brief Fatal check for exact equality of two Eigen matrix-like objects.
Expand All @@ -148,7 +148,7 @@ AssertionResult AssertMatrixNearHelper(
* @param[in] v2 The actual matrix
*/
#define ASSERT_MATRIX_EQ(v1, v2) \
GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_)
GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_)

/**
* @brief Non-fatal check for approximate equality of two Eigen matrix-like objects.
Expand All @@ -160,7 +160,7 @@ AssertionResult AssertMatrixNearHelper(
* @param[in] tol The allowed tolerance between any entries in v1 and v2
*/
#define EXPECT_MATRIX_NEAR(v1, v2, tol) \
GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_)
GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_)

/**
* @brief Fatal check for approximate equality of two Eigen matrix-like objects.
Expand All @@ -172,7 +172,7 @@ AssertionResult AssertMatrixNearHelper(
* @param[in] tol The allowed tolerance between any entries in v1 and v2
*/
#define ASSERT_MATRIX_NEAR(v1, v2, tol) \
GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_)
GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_)

} // namespace testing

Expand Down
Loading

0 comments on commit fe121e1

Please sign in to comment.