Fixes to build and use libRSF on Ubuntu 24.04 Noble

README.md

@@ -57,11 +57,11 @@ Alternatively, you can install them by your own:
   sudo apt-get install libeigen3-dev
   ```
 
-+ **Ceres** (>= 2.0) and its dependencies
++ **Ceres** (>= 2.1.0) and its dependencies
 
   ```bash
   sudo apt-get install libgoogle-glog-dev
-  sudo apt-get libgflags-dev
+  sudo apt-get install libgflags-dev
   sudo apt-get install libatlas-base-dev
   sudo apt-get install libsuitesparse-dev
 
@@ -85,7 +85,7 @@ Alternatively, you can install them by your own:
 + **GeographicLib**
 
   ```bash
-  sudo apt-get install libgeographic-dev
+  sudo apt-get install libgeographiclib-dev
   ```
 
 The library and its applications can be build following this instructions:

cmake/FindGeographicLib.cmake

@@ -1,12 +1,12 @@
 # Look for GeographicLib
 #
 # Set
-#  GeographicLib_FOUND = GEOGRAPHICLIB_FOUND = TRUE
+#  GeographicLib_FOUND = TRUE
 #  GeographicLib_INCLUDE_DIRS = /usr/local/include
-#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
+#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographicLib.so
 #  GeographicLib_LIBRARY_DIRS = /usr/local/lib
 
-find_library (GeographicLib_LIBRARIES Geographic
+find_library (GeographicLib_LIBRARIES GeographicLib
   PATHS "${CMAKE_INSTALL_PREFIX}/../GeographicLib/lib")
 
 if (GeographicLib_LIBRARIES)
@@ -17,7 +17,7 @@ if (GeographicLib_LIBRARIES)
   set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
   if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
     # On Debian systems the library is in e.g.,
-    #   /usr/lib/x86_64-linux-gnu/libGeographic.so
+    #   /usr/lib/x86_64-linux-gnu/libGeographicLib.so
     # so try stripping another element off _ROOT_DIR
     get_filename_component (_ROOT_DIR "${_ROOT_DIR}" PATH)
     set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
@@ -32,6 +32,7 @@ if (GeographicLib_LIBRARIES)
   unset (_ROOT_DIR)
 endif ()
 
+# This sets GeographicLib_FOUND (and GEOGRAPHICLIB_FOUND, deprecated) to TRUE
 include (FindPackageHandleStandardArgs)
 find_package_handle_standard_args (GeographicLib DEFAULT_MSG
   GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES

include/FactorGraph.h

@@ -439,7 +439,7 @@ namespace libRSF
       /** default settings for new ceres::Problems, especially enable_fast_removal = true */
       const ceres::Problem::Options DefaultProblemOptions_ = {ceres::Ownership::TAKE_OWNERSHIP, // cost_function_ownership
                                                               ceres::Ownership::TAKE_OWNERSHIP, // loss_function_ownership
-                                                              ceres::Ownership::TAKE_OWNERSHIP, // local_parameterization_ownership
+                                                              ceres::Ownership::TAKE_OWNERSHIP, // manifold_ownership
                                                               true, // enable_fast_removal
                                                               false, // disable_all_safety_checks
                                                               nullptr, // context

include/LocalParametrization.h

@@ -36,7 +36,8 @@
 #include "VectorMath.h"
 #include "Geometry.h"
 
-#include <ceres/local_parameterization.h>
+#include <ceres/manifold.h>
+#include <ceres/ceres.h>
 
 namespace libRSF
 {
@@ -46,15 +47,22 @@ namespace libRSF
     public:
 
       template <typename T>
-      bool operator()(const T* Angle, const T* DeltaAngle, T* AnglePlusDelta) const
+      bool Plus(const T* Angle, const T* DeltaAngle, T* AnglePlusDelta) const
       {
         *AnglePlusDelta = NormalizeAngle(*Angle + *DeltaAngle);
         return true;
       }
 
-      static ceres::LocalParameterization* Create()
+      template <typename T>
+      bool Minus(const T* y, const T* x, T* y_minus_x) const
+      {
+        *y_minus_x = NormalizeAngle(*y) - NormalizeAngle(*x);
+        return true;
+      }
+
+      static ceres::Manifold* Create()
       {
-        return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
+        return (new ceres::AutoDiffManifold<AngleLocalParameterization, 1, 1>);
       }
   };
 
@@ -64,7 +72,7 @@ namespace libRSF
     public:
 
       template <typename T>
-      bool operator()(const T* Circle, const T* DeltaCircle, T* CirclePlusDelta) const
+      bool Plus(const T* Circle, const T* DeltaCircle, T* CirclePlusDelta) const
       {
         /** real part --> cos() */
         CirclePlusDelta[0] = Circle[0] * cos(DeltaCircle[0]) - Circle[1] * sin(DeltaCircle[0]);
@@ -75,9 +83,17 @@ namespace libRSF
         return true;
       }
 
-      static ceres::LocalParameterization* Create()
+      template <typename T>
+      bool Minus(const T* y, const T* x, T* y_minus_x) const
       {
-        return (new ceres::AutoDiffLocalParameterization<UnitCircleLocalParameterization, 2, 1>);
+        /** extract angle from unit circle representation [cos(θ), sin(θ)] and compute normalized difference */
+        *y_minus_x = NormalizeAngle(ceres::atan2(y[1], y[0]) - ceres::atan2(x[1], x[0]));
+        return true;
+      }
+
+      static ceres::Manifold* Create()
+      {
+        return (new ceres::AutoDiffManifold<UnitCircleLocalParameterization, 2, 1>);
       }
   };
 
@@ -87,7 +103,7 @@ namespace libRSF
     public:
 
       template <typename T>
-      bool operator()(const T* Quaternion, const T* Delta, T* QuaternionPlusDelta) const
+      bool Plus(const T* Quaternion, const T* Delta, T* QuaternionPlusDelta) const
       {
         QuaternionRefConst<T> Q(Quaternion);
         VectorRefConst<T, 3> DeltaVec(Delta);
@@ -114,9 +130,22 @@ namespace libRSF
         return true;
       }
 
-      static ceres::LocalParameterization* Create()
+      template <typename T>
+      bool Minus(const T* y, const T* x, T* y_minus_x) const
+      {
+        QuaternionRefConst<T> Qy(y);
+        QuaternionRefConst<T> Qx(x);
+        VectorRef<T, 3> DeltaVec(y_minus_x);
+
+        /** compute quaternion difference in tangent space using log map */
+        DeltaVec = QuaternionError<T>(Qy, Qx);
+
+        return true;
+      }
+
+      static ceres::Manifold* Create()
       {
-        return (new ceres::AutoDiffLocalParameterization<QuaternionLocalParameterization, 4, 3>);
+        return (new ceres::AutoDiffManifold<QuaternionLocalParameterization, 4, 3>);
       }
   };
 }

include/libRSF.h

@@ -32,6 +32,8 @@
 #ifndef LIBRSF_H_INCLUDED
 #define LIBRSF_H_INCLUDED
 
+#include <glog/logging.h>
+
 /** most important functions */
 #include "FactorGraph.h"
 #include "FactorGraphConfig.h"

src/CMakeLists.txt

@@ -105,7 +105,7 @@ target_include_directories(libRSF
 )
 
 # set target (libRSF) dependencies
-target_link_libraries(libRSF PUBLIC Threads::Threads Eigen3::Eigen Ceres::ceres yaml-cpp ${GeographicLib_LIBRARIES})
+target_link_libraries(libRSF PUBLIC Threads::Threads Eigen3::Eigen Ceres::ceres yaml-cpp ${GeographicLib_LIBRARIES} glog)
 
 # enable all warnings for libRSF (this is just enabled from time to time to check the code quality)
 #target_compile_options(libRSF PUBLIC -Wextra -Wpedantic -Wall -fmax-errors=100 -Wno-unused-parameter)

src/FactorGraph.cpp

@@ -118,7 +118,7 @@ namespace libRSF
 
       case DataType::Pose2:
         {
-          ceres::LocalParameterization *LocalParamPose2 = new ceres::ProductParameterization(new ceres::IdentityParameterization(2), AngleLocalParameterization::Create());
+          ceres::Manifold *LocalParamPose2 = new ceres::ProductManifold(new ceres::EuclideanManifold<2>(), AngleLocalParameterization::Create());
           Graph_.AddParameterBlock(StatePointer, StateSize, LocalParamPose2);
           break;
         }
@@ -130,7 +130,7 @@ namespace libRSF
           Pose3 << 0,0,0, 0,0,0,1;
           StateData_.getElement(Name, Timestamp, StateNumber).setMean(Pose3);
 
-          ceres::LocalParameterization *LocalParamPose3 = new ceres::ProductParameterization(new ceres::IdentityParameterization(3), QuaternionLocalParameterization::Create());
+          ceres::Manifold *LocalParamPose3 = new ceres::ProductManifold(new ceres::EuclideanManifold<3>(), QuaternionLocalParameterization::Create());
           Graph_.AddParameterBlock(StatePointer, StateSize, LocalParamPose3);
           break;
         }
@@ -211,9 +211,9 @@ namespace libRSF
 
   void FactorGraph::setSubsetConstant(const string& Name, const double Timestamp, const int Number, const std::vector<int> &ConstantIndex)
   {
-    Graph_.SetParameterization(
+    Graph_.SetManifold(
         StateData_.getElement(Name, Timestamp, Number).getMeanPointer(),
-                               new ceres::SubsetParameterization(
+                               new ceres::SubsetManifold(
             StateData_.getElement(Name, Timestamp, Number).getMean().size(), ConstantIndex));
   }
 
@@ -283,7 +283,7 @@ namespace libRSF
           StateData_.getElement(State.ID, State.Timestamp, State.Number).getMeanPointer());
 
       /** compute size of the marginalized system */
-      MarginalSize += Graph_.ParameterBlockLocalSize(MarginalStates.back());
+      MarginalSize += Graph_.ParameterBlockTangentSize(MarginalStates.back());
     }
 
     /** get connected states */

src/FactorGraphStructure.cpp

@@ -97,7 +97,7 @@ namespace libRSF
     {
       ConnectedStates.emplace_back(State);
       StateDims.emplace_back(Graph_->ParameterBlockSize(State));
-      StateDimsLocal.emplace_back(Graph_->ParameterBlockLocalSize(State));
+      StateDimsLocal.emplace_back(Graph_->ParameterBlockTangentSize(State));
 
       /** find state info */
       StateInfo Info = States_.at(State);