locusrobotics · svwilliams · Apr 3, 2024
diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt
@@ -29,6 +29,47 @@ include(suitesparse-extras.cmake)
 ###########
 ## Build ##
 ###########
+# lint_cmake: -linelength
+# Disable warnings about array bounds with -Wno-array-bounds until gcc 12 fixes this bug:
+# https://gcc.gnu.org/bugzilla/show_bug.cgi?id=106247
+#
+# Also reported in Eigen, and confirmed to be due to gcc 12:
+# https://gitlab.com/libeigen/eigen/-/issues/2506
+#
+# In file included from include/immintrin.h:43,
+#                  from include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:361,
+#                  from include/eigen3/Eigen/Core:22,
+#                  from include/fuse_core/fuse_macros.h:63,
+#                  from include/fuse_core/loss.h:37,
+#                  from include/fuse_core/constraint.h:37,
+#                  from src/fuse/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h:37,
+#                  from src/fuse/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp:34:
+# In function ‘__m256d _mm256_loadu_pd(const double*)’,
+#     inlined from ‘Packet Eigen::internal::ploadu(const typename unpacket_traits<T>::type*) [with Packet = __vector(4) double]’ at include/eigen3/Eigen/src/Core/arch/AVX/PacketMath.h:582:129,
+#     inlined from ‘Packet Eigen::internal::ploadt(const typename unpacket_traits<T>::type*) [with Packet = __vector(4) double; int Alignment = 0]’ at include/eigen3/Eigen/src/Core/GenericPacketMath.h:969:26,
+#     inlined from ‘PacketType Eigen::internal::evaluator<Eigen::PlainObjectBase<Derived> >::packet(Eigen::Index) const [with int LoadMode = 0; PacketType = __vector(4) double; Derived = Eigen::Matrix<double, 3, 1>]’ at include/eigen3/Eigen/src/Core/CoreEvaluators.h:245:40,
+#     inlined from ‘void Eigen::internal::generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, Version>::assignPacket(Eigen::Index) [with int StoreMode = 32; int LoadMode = 0; PacketType = __vector(4) double; DstEvaluatorTypeT = Eigen::internal::evaluator<Eigen::Map<Eigen::Matrix<double, -1, 1> > >; SrcEvaluatorTypeT = Eigen::internal::evaluator<Eigen::Matrix<double, 3, 1> >; Functor = Eigen::internal::assign_op<double, double>; int Version = 0]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:681:114,
+#     inlined from ‘static void Eigen::internal::dense_assignment_loop<Kernel, 3, 0>::run(Kernel&) [with Kernel = Eigen::internal::generic_dense_assignment_kernel<Eigen::internal::evaluator<Eigen::Map<Eigen::Matrix<double, -1, 1> > >, Eigen::internal::evaluator<Eigen::Matrix<double, 3, 1> >, Eigen::internal::assign_op<double, double>, 0>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:437:75,
+#     inlined from ‘void Eigen::internal::call_dense_assignment_loop(DstXprType&, const SrcXprType&, const Functor&) [with DstXprType = Eigen::Map<Eigen::Matrix<double, -1, 1> >; SrcXprType = Eigen::Matrix<double, 3, 1>; Functor = assign_op<double, double>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:785:37,
+#     inlined from ‘static void Eigen::internal::Assignment<DstXprType, SrcXprType, Functor, Eigen::internal::Dense2Dense, Weak>::run(DstXprType&, const SrcXprType&, const Functor&) [with DstXprType = Eigen::Map<Eigen::Matrix<double, -1, 1> >; SrcXprType = Eigen::Matrix<double, 3, 1>; Functor = Eigen::internal::assign_op<double, double>; Weak = void]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:954:31,
+#     inlined from ‘void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Map<Eigen::Matrix<double, -1, 1> >; Src = Eigen::Matrix<double, 3, 1>; Func = assign_op<double, double>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:890:49,
+#     inlined from ‘void Eigen::internal::call_assignment(Dst&, const Src&, const Func&, typename enable_if<evaluator_assume_aliasing<Src>::value, void*>::type) [with Dst = Eigen::Map<Eigen::Matrix<double, -1, 1> >; Src = Eigen::Product<Eigen::Matrix<double, 3, 3, 1>, Eigen::Map<Eigen::Matrix<double, -1, 1> >, 0>; Func = assign_op<double, double>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:851:27,
+#     inlined from ‘void Eigen::internal::call_assignment(Dst&, const Src&) [with Dst = Eigen::Map<Eigen::Matrix<double, -1, 1> >; Src = Eigen::Product<Eigen::Matrix<double, 3, 3, 1>, Eigen::Map<Eigen::Matrix<double, -1, 1> >, 0>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:836:18,
+#     inlined from ‘Derived& Eigen::MatrixBase<Derived>::operator=(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Product<Eigen::Matrix<double, 3, 3, 1>, Eigen::Map<Eigen::Matrix<double, -1, 1> >, 0>; Derived = Eigen::Map<Eigen::Matrix<double, -1, 1> >]’ at include/eigen3/Eigen/src/Core/Assign.h:66:28,
+#     inlined from ‘void Eigen::EigenBase<Derived>::applyThisOnTheLeft(Dest&) const [with Dest = Eigen::Map<Eigen::Matrix<double, -1, 1> >; Derived = Eigen::Matrix<double, 3, 3, 1>]’ at include/eigen3/Eigen/src/Core/EigenBase.h:114:9:
+# include/avxintrin.h:893:24: error: array subscript ‘__m256d_u[0]’ is partly outside array bounds of ‘Eigen::internal::plain_matrix_type<Eigen::Product<Eigen::Matrix<double, 3, 3, 1>, Eigen::Map<Eigen::Matrix<double, -1, 1> >, 0>, Eigen::Dense>::type [1]’ {aka ‘Eigen::Matrix<double, 3, 1> [1]’} [-Werror=array-bounds]
+#   893 |   return *(__m256d_u *)__P;
+#       |                        ^~~
+# In file included from include/eigen3/Eigen/Core:278:
+# include/eigen3/Eigen/src/Core/AssignEvaluator.h: In member function ‘void Eigen::EigenBase<Derived>::applyThisOnTheLeft(Dest&) const [with Dest = Eigen::Map<Eigen::Matrix<double, -1, 1> >; Derived = Eigen::Matrix<double, 3, 3, 1>]’:
+# include/eigen3/Eigen/src/Core/AssignEvaluator.h:850:41: note: at offset [0, 16] into object ‘tmp’ of size 24
+#   850 |   typename plain_matrix_type<Src>::type tmp(src);
+#       |                                         ^~~
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 12.0)
+  add_compile_options(-Wall -Werror -Wno-array-bounds)
+else()
+  add_compile_options(-Wall -Werror)
+endif()
 
 # fuse_constraints library
 add_library(${PROJECT_NAME}
@@ -116,7 +157,6 @@ ament_export_dependencies(
   geometry_msgs
   pluginlib
   rclcpp
-
   Ceres
   Eigen3
   glog

diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp
@@ -38,14 +38,21 @@
 
 #include <algorithm>
 #include <cassert>
+#include <functional>
+#include <iterator>
 #include <ostream>
+#include <stdexcept>
 #include <string>
+#include <utility>
 #include <vector>
 
+#include <fuse_core/ceres_macros.hpp>
 #include <fuse_core/constraint.hpp>
 #include <fuse_core/eigen.hpp>
-#include <fuse_core/local_parameterization.hpp>
 #include <fuse_core/fuse_macros.hpp>
+#include <fuse_core/local_parameterization.hpp>
+#include <fuse_core/manifold.hpp>
+#include <fuse_core/manifold_adapter.hpp>
 #include <fuse_core/serialization.hpp>
 #include <fuse_core/variable.hpp>
 
@@ -125,13 +132,20 @@ class MarginalConstraint : public fuse_core::Constraint
    */
   const std::vector<fuse_core::VectorXd> & x_bar() const {return x_bar_;}
 
+#if !CERES_SUPPORTS_MANIFOLDS
   /**
    * @brief Read-only access to the variable local parameterizations
    */
   const std::vector<fuse_core::LocalParameterization::SharedPtr> & localParameterizations() const
   {
     return local_parameterizations_;
   }
+#else
+  /**
+   * @brief Read-only access to the variable manifolds
+   */
+  const std::vector<fuse_core::Manifold::SharedPtr> & manifolds() const {return manifolds_;}
+#endif
 
   /**
    * @brief Print a human-readable description of the constraint to the provided stream.
@@ -155,32 +169,88 @@ class MarginalConstraint : public fuse_core::Constraint
 protected:
   std::vector<fuse_core::MatrixXd> A_;  //!< The A matrices of the marginal constraint
   fuse_core::VectorXd b_;  //!< The b vector of the marginal constraint
-
+#if !CERES_SUPPORTS_MANIFOLDS
   //!< The local parameterizations
   std::vector<fuse_core::LocalParameterization::SharedPtr> local_parameterizations_;
-
+#else
+  std::vector<fuse_core::Manifold::SharedPtr> manifolds_;  //!< Manifolds
+#endif
   std::vector<fuse_core::VectorXd> x_bar_;  //!< The linearization point of each involved variable
 
 private:
   // Allow Boost Serialization access to private methods
   friend class boost::serialization::access;
 
   /**
-   * @brief The Boost Serialize method that serializes all of the data members in to/out of the
-   *        archive
+   * @brief The Boost Serialize method that serializes all of the data members in to 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 */)
+  void save(Archive & archive, const unsigned int /* version */) const
   {
-    archive & boost::serialization::base_object<fuse_core::Constraint>(*this);
-    archive & A_;
-    archive & b_;
-    archive & local_parameterizations_;
-    archive & x_bar_;
+    archive << boost::serialization::base_object<fuse_core::Constraint>(*this);
+    archive << A_;
+    archive << b_;
+#if !CERES_SUPPORTS_MANIFOLDS
+    archive << local_parameterizations_;
+#else
+    archive << manifolds_;
+#endif
+    archive << x_bar_;
   }
+
+  /**
+   * @brief The Boost Serialize method that serializes all of the data members 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
@@ -202,14 +272,24 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable & var
   return Eigen::Map<const fuse_core::VectorXd>(variable.data(), variable.size());
 }
 
+#if !CERES_SUPPORTS_MANIFOLDS
 /**
  * @brief Return the local parameterization of the provided variable
  */
-inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterization(
+inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(
   const fuse_core::Variable & variable)
 {
   return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization());
 }
+#else
+/**
+ * @brief Return the manifold of the provided variable
+ */
+inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable & variable)
+{
+  return fuse_core::Manifold::SharedPtr(variable.manifold());
+}
+#endif
 
 }  // namespace detail
 
@@ -226,16 +306,26 @@ MarginalConstraint::MarginalConstraint(
     boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)),
   A_(first_A, last_A),
   b_(b),
+#if !CERES_SUPPORTS_MANIFOLDS
   local_parameterizations_(boost::make_transform_iterator(first_variable,
     &fuse_constraints::detail::getLocalParameterization),
     boost::make_transform_iterator(last_variable,
     &fuse_constraints::detail::getLocalParameterization)),
+#else
+  manifolds_(
+    boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getManifold),
+    boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getManifold)),
+#endif
   x_bar_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getCurrentValue),
     boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue))
 {
   assert(!A_.empty());
   assert(A_.size() == x_bar_.size());
+#if !CERES_SUPPORTS_MANIFOLDS
   assert(A_.size() == local_parameterizations_.size());
+#else
+  assert(A_.size() == manifolds_.size());
+#endif
   assert(b_.rows() > 0);
   assert(
     std::all_of(
@@ -255,5 +345,13 @@ 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_HPP_
diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp
@@ -38,9 +38,10 @@
 
 #include <vector>
 
+#include <fuse_core/ceres_macros.hpp>
 #include <fuse_core/eigen.hpp>
 #include <fuse_core/local_parameterization.hpp>
-
+#include <fuse_core/manifold.hpp>
 
 namespace fuse_constraints
 {
@@ -66,6 +67,7 @@ namespace fuse_constraints
 class MarginalCostFunction : public ceres::CostFunction
 {
 public:
+#if !CERES_SUPPORTS_MANIFOLDS
   /**
    * @brief Construct a cost function instance
    *
@@ -81,6 +83,21 @@ class MarginalCostFunction : public ceres::CostFunction
     const fuse_core::VectorXd & b,
     const std::vector<fuse_core::VectorXd> & x_bar,
     const std::vector<fuse_core::LocalParameterization::SharedPtr> & local_parameterizations);
+#else
+  /**
+   * @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
+   */
+  MarginalCostFunction(
+    const std::vector<fuse_core::MatrixXd> & A,
+    const fuse_core::VectorXd & b,
+    const std::vector<fuse_core::VectorXd> & x_bar,
+    const std::vector<fuse_core::Manifold::SharedPtr> & manifolds);
+#endif
 
   /**
    * @brief Destructor
@@ -99,10 +116,12 @@ class MarginalCostFunction : public ceres::CostFunction
 private:
   const std::vector<fuse_core::MatrixXd> & A_;  //!< The A matrices of the marginal cost
   const fuse_core::VectorXd & b_;  //!< The b vector of the marginal cost
-
+#if !CERES_SUPPORTS_MANIFOLDS
   //!< Parameterizations
   const std::vector<fuse_core::LocalParameterization::SharedPtr> & local_parameterizations_;
-
+#else
+  const std::vector<fuse_core::Manifold::SharedPtr> & manifolds_;  //!< Manifolds
+#endif
   const std::vector<fuse_core::VectorXd> & x_bar_;  //!< The linearization point of each variable
 };
 

diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp
@@ -47,9 +47,8 @@
 #include <fuse_constraints/uuid_ordering.hpp>
 #include <fuse_core/constraint.hpp>
 #include <fuse_core/eigen.hpp>
-#include <fuse_core/graph.hpp>
-#include <fuse_core/local_parameterization.hpp>
 #include <fuse_core/fuse_macros.hpp>
+#include <fuse_core/graph.hpp>
 #include <fuse_core/transaction.hpp>
 #include <fuse_core/variable.hpp>
 

diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp
@@ -38,6 +38,7 @@
 #include <boost/serialization/export.hpp>
 #include <fuse_constraints/marginal_constraint.hpp>
 #include <fuse_constraints/marginal_cost_function.hpp>
+#include <fuse_core/ceres_macros.hpp>
 #include <fuse_core/constraint.hpp>
 #include <pluginlib/class_list_macros.hpp>
 
@@ -68,7 +69,11 @@ void MarginalConstraint::print(std::ostream & stream) const
 
 ceres::CostFunction * MarginalConstraint::costFunction() const
 {
+#if !CERES_SUPPORTS_MANIFOLDS
   return new MarginalCostFunction(A_, b_, x_bar_, local_parameterizations_);
+#else
+  return new MarginalCostFunction(A_, b_, x_bar_, manifolds_);
+#endif
 }
 
 }  // namespace fuse_constraints