Port support for Ceres 2.1.0 Manifold class into ROS 2 Rolling (locusrobotics#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

Author: svwilliams

fuse_constraints/src/marginalize_variables.cpp

@@ -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;

fuse_constraints/src/variable_constraints.cpp

@@ -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);
 }
 

fuse_core/include/fuse_core/callback_wrapper.hpp

@@ -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

fuse_core/include/fuse_core/ceres_options.hpp

@@ -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)
 /**
@@ -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";
   }
@@ -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";
   }

fuse_core/include/fuse_core/constraint.hpp

@@ -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
@@ -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
@@ -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()
@@ -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()
@@ -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

fuse_core/include/fuse_core/eigen_gtest.hpp

@@ -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
 
@@ -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.
@@ -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.
@@ -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.
@@ -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