Support gcc12 and ceres 2.1.0 (#341)

Add support for the Manifold class when using Ceres Solver version 2.1.0 and above --------- Co-authored-by: Jake McLaughlin <[email protected]>
locusrobotics · Apr 3, 2024 · 522a2ec · 522a2ec
1 parent 415971a
commit 522a2ec
Show file tree

Hide file tree

Showing 45 changed files with 1,643 additions and 813 deletions.
diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt
@@ -35,7 +35,47 @@ catkin_package(
 ###########
 ## Build ##
 ###########
-add_compile_options(-Wall -Werror)
+
+# 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}
@@ -66,6 +106,9 @@ add_dependencies(${PROJECT_NAME}
 target_include_directories(${PROJECT_NAME}
   PUBLIC
     include
+)
+target_include_directories(${PROJECT_NAME}
+  SYSTEM PUBLIC
     ${catkin_INCLUDE_DIRS}
     ${CERES_INCLUDE_DIRS}
     ${EIGEN3_INCLUDE_DIRS}

diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.h b/fuse_constraints/include/fuse_constraints/marginal_constraint.h
@@ -36,8 +36,9 @@
 
 #include <fuse_core/constraint.h>
 #include <fuse_core/eigen.h>
-#include <fuse_core/local_parameterization.h>
 #include <fuse_core/fuse_macros.h>
+#include <fuse_core/local_parameterization.h>
+#include <fuse_core/manifold.h>
 #include <fuse_core/serialization.h>
 #include <fuse_core/variable.h>
 
@@ -57,10 +58,8 @@
 #include <string>
 #include <vector>
 
-
 namespace fuse_constraints
 {
-
 /**
  * @brief A constraint that represents remaining marginal information on a set of variables
  *
@@ -93,7 +92,7 @@ class MarginalConstraint : public fuse_core::Constraint
    * @param[in] last_A         Iterator pointing to one past the last A matrix
    * @param[in] b              The b vector of the marginal cost (of the form A*(x - x_bar) + b)
    */
-  template<typename VariableIterator, typename MatrixIterator>
+  template <typename VariableIterator, typename MatrixIterator>
   MarginalConstraint(
     const std::string& source,
     VariableIterator first_variable,
@@ -122,13 +121,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 local parameterizations
+   */
+  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.
@@ -151,7 +157,11 @@ 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
-  std::vector<fuse_core::LocalParameterization::SharedPtr> local_parameterizations_;  //!< The local parameterizations
+#if !CERES_SUPPORTS_MANIFOLDS
+  std::vector<fuse_core::LocalParameterization::SharedPtr> local_parameterizations_;  //!< 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:
@@ -164,20 +174,23 @@ class MarginalConstraint : public fuse_core::Constraint
    * @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>
+  template <class Archive>
   void serialize(Archive& archive, const unsigned int /* version */)
   {
-    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_;
   }
 };
 
 namespace detail
 {
-
 /**
  * @brief Return the UUID of the provided variable
  */
@@ -194,6 +207,7 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable& vari
   return Eigen::Map<const fuse_core::VectorXd>(variable.data(), variable.size());
 }
 
+#if !CERES_SUPPORTS_MANIFOLDS
 /**
  * @brief Return the local parameterization of the provided variable
  */
@@ -202,31 +216,52 @@ inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterizatio
   return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization());
 }
 
+#else
+/**
+ * @brief Return the manifold of the provided variable
+ */
+inline fuse_core::Manifold::SharedPtr const getManifold(const fuse_core::Variable& variable)
+{
+  return fuse_core::Manifold::SharedPtr(variable.manifold());
+}
+#endif
+
 }  // namespace detail
 
-template<typename VariableIterator, typename MatrixIterator>
+template <typename VariableIterator, typename MatrixIterator>
 MarginalConstraint::MarginalConstraint(
   const std::string& source,
   VariableIterator first_variable,
   VariableIterator last_variable,
   MatrixIterator first_A,
   MatrixIterator last_A,
   const fuse_core::VectorXd& b) :
-    Constraint(source,
-               boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getUuid),
-               boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)),
-    A_(first_A, last_A),
-    b_(b),
-    local_parameterizations_(boost::make_transform_iterator(first_variable,
-                                                            &fuse_constraints::detail::getLocalParameterization),
-                             boost::make_transform_iterator(last_variable,
-                                                            &fuse_constraints::detail::getLocalParameterization)),
-    x_bar_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getCurrentValue),
-           boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue))
+  Constraint(
+    source,
+    boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getUuid),
+    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(A_.begin(), A_.end(), [this](const auto& A){ return A.rows() == this->b_.rows(); }));  // NOLINT
   assert(std::all_of(boost::make_zip_iterator(boost::make_tuple(A_.begin(), first_variable)),

diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.h b/fuse_constraints/include/fuse_constraints/marginal_cost_function.h
@@ -36,15 +36,14 @@
 
 #include <fuse_core/eigen.h>
 #include <fuse_core/local_parameterization.h>
+#include <fuse_core/manifold.h>
 
 #include <ceres/cost_function.h>
 
 #include <vector>
 
-
 namespace fuse_constraints
 {
-
 /**
  * @brief Implements a cost function designed for precomputed marginal distributions
  *
@@ -56,14 +55,15 @@ namespace fuse_constraints
  *
  * where, the A matrices and the b vector are fixed, x_bar is the linearization point used when calculating the A
  * matrices and b vector, and the minus operator in (x - x_bar) is provided by the variable's local parameterization.
- * 
+ *
  * The A matrices can have any number of rows, but they must all be the same. The number of columns of each A matrix
  * must match the associated variable's local parameterization size, and the number of rows of each x_bar must match
  * the associated variable's global size. The cost function will have the same number of residuals as the rows of A.
  */
 class MarginalCostFunction : public ceres::CostFunction
 {
 public:
+#if !CERES_SUPPORTS_MANIFOLDS
   /**
    * @brief Construct a cost function instance
    *
@@ -77,6 +77,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
@@ -87,15 +102,16 @@ 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
   const fuse_core::VectorXd& b_;  //!< The b vector of the marginal cost
+#if !CERES_SUPPORTS_MANIFOLDS
   const std::vector<fuse_core::LocalParameterization::SharedPtr>& local_parameterizations_;  //!< 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.h b/fuse_constraints/include/fuse_constraints/marginalize_variables.h
@@ -38,9 +38,8 @@
 #include <fuse_constraints/uuid_ordering.h>
 #include <fuse_core/constraint.h>
 #include <fuse_core/eigen.h>
-#include <fuse_core/graph.h>
-#include <fuse_core/local_parameterization.h>
 #include <fuse_core/fuse_macros.h>
+#include <fuse_core/graph.h>
 #include <fuse_core/transaction.h>
 #include <fuse_core/variable.h>
 
@@ -54,10 +53,8 @@
 #include <string>
 #include <vector>
 
-
 namespace fuse_constraints
 {
-
 /**
  * @brief Compute an efficient elimination order for the marginalized variables
  *
@@ -129,7 +126,6 @@ fuse_core::Transaction marginalizeVariables(
 
 namespace detail
 {
-
 /**
  * @brief Structure holding linearized Jacobian blocks
  *

diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp
@@ -37,15 +37,13 @@
 #include <fuse_core/constraint.h>
 #include <pluginlib/class_list_macros.hpp>
 
-#include <boost/serialization/export.hpp>
 #include <Eigen/Core>
+#include <boost/serialization/export.hpp>
 
 #include <ostream>
 
-
 namespace fuse_constraints
 {
-
 void MarginalConstraint::print(std::ostream& stream) const
 {
   stream << type() << "\n"
@@ -59,8 +57,10 @@ 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";
 
@@ -73,7 +73,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