Gcc12 ceres 2.1.0 manifold locus (#363)

* General clean up for Ceres 2.2.0 support
* Updated serialization support to be backwards compatible with previously serialized files

---------

Co-authored-by: Enrique Fernandez Perdomo <[email protected]>

Author: svwilliams

fuse_constraints/CMakeLists.txt

@@ -336,6 +336,7 @@ if(CATKIN_ENABLE_TESTING)
   # Marginal Constraint Tests
   catkin_add_gtest(test_marginal_constraint
     test/test_marginal_constraint.cpp
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test
   )
   add_dependencies(test_marginal_constraint
     ${catkin_EXPORTED_TARGETS}

fuse_constraints/include/fuse_constraints/marginal_constraint.h

@@ -39,6 +39,7 @@
 #include <fuse_core/fuse_macros.h>
 #include <fuse_core/local_parameterization.h>
 #include <fuse_core/manifold.h>
+#include <fuse_core/manifold_adapter.h>
 #include <fuse_core/serialization.h>
 #include <fuse_core/variable.h>
 
@@ -48,14 +49,19 @@
 #include <boost/serialization/base_object.hpp>
 #include <boost/serialization/export.hpp>
 #include <boost/serialization/shared_ptr.hpp>
+#include <boost/serialization/unique_ptr.hpp>
 #include <boost/serialization/vector.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <ceres/cost_function.h>
 
 #include <algorithm>
 #include <cassert>
+#include <functional>
+#include <iterator>
 #include <ostream>
+#include <stdexcept>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace fuse_constraints
@@ -131,7 +137,7 @@ class MarginalConstraint : public fuse_core::Constraint
   }
 #else
   /**
-   * @brief Read-only access to the variable local parameterizations
+   * @brief Read-only access to the variable manifolds
    */
   const std::vector<fuse_core::Manifold::SharedPtr>& manifolds() const { return manifolds_; }
 #endif
@@ -171,22 +177,72 @@ class MarginalConstraint : public fuse_core::Constraint
   /**
    * @brief The Boost Serialize method that serializes all of the data members in to/out of the archive
    *
-   * @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.
+   * @param[out] archive - The archive object into which class members will be serialized
+   * @param[in] version - The version of the archive being written.
    */
-  template <class Archive>
-  void serialize(Archive& archive, const unsigned int /* version */)
+  template<class Archive>
+  void save(Archive& archive, const unsigned int version) const
   {
-    archive& boost::serialization::base_object<fuse_core::Constraint>(*this);
-    archive& A_;
-    archive& b_;
+    archive << boost::serialization::base_object<fuse_core::Constraint>(*this);
+    archive << A_;
+    archive << b_;
 #if !CERES_SUPPORTS_MANIFOLDS
-    archive& local_parameterizations_;
+      archive << local_parameterizations_;
 #else
-    archive& manifolds_;
+      archive << manifolds_;
 #endif
-    archive& x_bar_;
+    archive << x_bar_;
   }
+
+  /**
+   * @brief The Boost Serialize method that serializes all of the data members in to/out of the archive
+   *
+   * @param[in] archive - The archive object that holds the serialized class members
+   * @param[in] version - The version of the archive being read.
+   */
+  template<class Archive>
+  void load(Archive& archive, const unsigned int version)
+  {
+    archive >> boost::serialization::base_object<fuse_core::Constraint>(*this);
+    archive >> A_;
+    archive >> b_;
+    if (version == 0)
+    {
+      // Version 0 serialization files will contain a std::vector of LocalParameterization shared pointers.
+      // If the current version of Ceres Solver does not support Manifolds, then the serialized LocalParameterization
+      // pointers can be deserialized directly into the class member.
+      // But if the current version of Ceres Solver supports manifolds, then the serialized LocalParameterization
+      // pointers must be wrapped in a Manifold adapter first.
+#if !CERES_SUPPORTS_MANIFOLDS
+      archive >> local_parameterizations_;
+#else
+      auto local_parameterizations = std::vector<fuse_core::LocalParameterization::SharedPtr>();
+      archive >> local_parameterizations;
+      std::transform(
+        std::make_move_iterator(local_parameterizations.begin()),
+        std::make_move_iterator(local_parameterizations.end()),
+        std::back_inserter(manifolds_),
+        [](fuse_core::LocalParameterization::SharedPtr local_parameterization)
+        { return fuse_core::ManifoldAdapter::make_shared(std::move(local_parameterization)); });
+#endif
+    }
+    else  // (version >= 1)
+    {
+      // Version 1 serialization files will contain a std::vector of Manifold shared pointers. If the current version
+      // of Ceres Solver does not support Manifolds, then there is no way to deserialize the requested data.
+      // But if the current version of Ceres Solver does support manifolds, then the serialized Manifold pointers
+      // can be deserialized directly into the class member.
+#if !CERES_SUPPORTS_MANIFOLDS
+      throw std::runtime_error("Attempting to deserialize an archive saved in Version " + std::to_string(version) +
+        " format. However, the current version of Ceres Solver (" + CERES_VERSION_STRING + ") does not support"
+        " manifolds. Ceres Solver version 2.1.0 or later is required to load this file.");
+#else
+      archive >> manifolds_;
+#endif
+    }
+    archive >> x_bar_;
+  }
+  BOOST_SERIALIZATION_SPLIT_MEMBER();
 };
 
 namespace detail
@@ -211,7 +267,7 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable& vari
 /**
  * @brief Return the local parameterization of the provided variable
  */
-inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterization(const fuse_core::Variable& variable)
+inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(const fuse_core::Variable& variable)
 {
   return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization());
 }
@@ -220,7 +276,7 @@ inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterizatio
 /**
  * @brief Return the manifold of the provided variable
  */
-inline fuse_core::Manifold::SharedPtr const getManifold(const fuse_core::Variable& variable)
+inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable& variable)
 {
   return fuse_core::Manifold::SharedPtr(variable.manifold());
 }
@@ -275,5 +331,12 @@ MarginalConstraint::MarginalConstraint(
 }  // namespace fuse_constraints
 
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::MarginalConstraint);
+// Since the contents of the serialized file will change depending on the CeresSolver version, also set the
+// Boost Serialization version to allow code reading serialized file to know what data to expect.
+#if !CERES_SUPPORTS_MANIFOLDS
+BOOST_CLASS_VERSION(fuse_constraints::MarginalConstraint, 0);
+#else
+BOOST_CLASS_VERSION(fuse_constraints::MarginalConstraint, 1);
+#endif
 
 #endif  // FUSE_CONSTRAINTS_MARGINAL_CONSTRAINT_H

fuse_constraints/include/fuse_constraints/marginal_cost_function.h

@@ -81,10 +81,10 @@ class MarginalCostFunction : public ceres::CostFunction
   /**
    * @brief Construct a cost function instance
    *
-   * @param[in] A                       The A matrix of the marginal cost (of the form A*(x - x_bar) + b)
-   * @param[in] b                       The b vector of the marginal cost (of the form A*(x - x_bar) + b)
-   * @param[in] x_bar                   The linearization point of the involved variables
-   * @param[in] manifolds               The manifold associated with the variable
+   * @param[in] A         The A matrix of the marginal cost (of the form A*(x - x_bar) + b)
+   * @param[in] b         The b vector of the marginal cost (of the form A*(x - x_bar) + b)
+   * @param[in] x_bar     The linearization point of the involved variables
+   * @param[in] manifolds The manifold associated with the variable
    */
   MarginalCostFunction(
     const std::vector<fuse_core::MatrixXd>& A,
@@ -102,7 +102,10 @@ class MarginalCostFunction : public ceres::CostFunction
    * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided variable/parameter
    *        values
    */
-  bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override;
+  bool Evaluate(
+    double const* const* parameters,
+    double* residuals,
+    double** jacobians) const override;
 
 private:
   const std::vector<fuse_core::MatrixXd>& A_;  //!< The A matrices of the marginal cost

fuse_constraints/src/marginal_constraint.cpp

@@ -57,10 +57,8 @@ void MarginalConstraint::print(std::ostream& stream) const
   Eigen::IOFormat indent(4, 0, ", ", "\n", "   [", "]");
   for (size_t i = 0; i < A().size(); ++i)
   {
-    stream << "  A[" << i << "]:\n"
-           << A()[i].format(indent) << "\n"
-           << "  x_bar[" << i << "]:\n"
-           << x_bar()[i].format(indent) << "\n";
+    stream << "  A[" << i << "]:\n" << A()[i].format(indent) << "\n"
+           << "  x_bar[" << i << "]:\n" << x_bar()[i].format(indent) << "\n";
   }
   stream << "  b:\n" << b().format(indent) << "\n";
 

fuse_constraints/src/marginal_cost_function.cpp

@@ -79,7 +79,10 @@ MarginalCostFunction::MarginalCostFunction(
 }
 #endif
 
-bool MarginalCostFunction::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+bool MarginalCostFunction::Evaluate(
+  double const* const* parameters,
+  double* residuals,
+  double** jacobians) const
 {
   // Compute cost
   Eigen::Map<fuse_core::VectorXd> residuals_map(residuals, num_residuals());

fuse_constraints/src/marginalize_variables.cpp

@@ -103,8 +103,10 @@ UuidOrdering computeEliminationOrder(
   }
 
   // Construct the CCOLAMD input structures
-  auto recommended_size =
-    ccolamd_recommended(variable_constraints.size(), constraint_order.size(), variable_order.size());
+  auto recommended_size = ccolamd_recommended(
+    variable_constraints.size(),
+    constraint_order.size(),
+    variable_order.size());
   auto A = std::vector<int>(recommended_size);
   auto p = std::vector<int>(variable_order.size() + 1);
 

fuse_constraints/test/legacy_marginal_version0.txt

@@ -0,0 +1,3 @@
+22 serialization::archive 17 1 0
+0 0 0 4 test 68a8f481-d9e2-434e-96d8-d9c52fac6603 0 0 1 0 d0344d9a-5584-5a24-b972-ea410b25e499 0 1 -1 0 0 1 0 0 0 1 3 5.00000000000000000e+00 6.00000000000000000e+00 7.00000000000000000e+00 0 0 1 1 8.00000000000000000e+00 0 0 1 1 0 1 9 50 fuse_variables::Orientation3DLocalParameterization 1 0
+1 0 0 0 0 1 0 4 1 8.42614976999999987e-01 2.00000000000000011e-01 2.99999999999999989e-01 4.00000000000000022e-01