From 24f7cbf6baef3d5ab3a4eafc2f379684755d315b Mon Sep 17 00:00:00 2001
From: Michael Ripperger <michael.ripperger@swri.org>
Date: Thu, 18 Jul 2024 14:57:19 -0500
Subject: [PATCH] Update to support Ceres versions >= 2.1.0

---
 .../dh_chain_kinematic_calibration_cost.h        |  6 ++++++
 .../optimizations/local_parameterization.h       | 12 ++++++++++++
 test/local_parameterization_utest.cpp            | 16 ++++++++++++++++
 3 files changed, 34 insertions(+)

diff --git a/optimizations/include/industrial_calibration/optimizations/dh_chain_kinematic_calibration_cost.h b/optimizations/include/industrial_calibration/optimizations/dh_chain_kinematic_calibration_cost.h
index 17648a9e..e211d41b 100644
--- a/optimizations/include/industrial_calibration/optimizations/dh_chain_kinematic_calibration_cost.h
+++ b/optimizations/include/industrial_calibration/optimizations/dh_chain_kinematic_calibration_cost.h
@@ -5,6 +5,8 @@
 #include <industrial_calibration/optimizations/ceres_math_utilities.h>
 
 #include <ceres/jet.h>
+#include <ceres/version.h>
+#define CERES_VERSION_LT_2_1 (CERES_VERSION_MAJOR < 2 || (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR < 1))
 #include <map>
 
 namespace industrial_calibration
@@ -280,7 +282,11 @@ class DualDHChainMeasurementCost : public DualDHChainCost
     T rot_diff = Eigen::Quaternion<T>(camera_to_target_measured_.cast<T>().linear())
                      .angularDistance(Eigen::Quaternion<T>(camera_to_target.linear()));
 
+#if CERES_VERSION_LT_2_1
     residual[3] = ceres::IsNaN(rot_diff) ? T(0.0) : T(orientation_weight_) * rot_diff;
+#else
+    residual[3] = ceres::isnan(rot_diff) ? T(0.0) : T(orientation_weight_) * rot_diff;
+#endif
 
     return true;
   }
diff --git a/optimizations/include/industrial_calibration/optimizations/local_parameterization.h b/optimizations/include/industrial_calibration/optimizations/local_parameterization.h
index 59fb9548..8305b263 100644
--- a/optimizations/include/industrial_calibration/optimizations/local_parameterization.h
+++ b/optimizations/include/industrial_calibration/optimizations/local_parameterization.h
@@ -4,7 +4,14 @@
 #include <industrial_calibration/core/exceptions.h>
 
 #include <ceres/problem.h>
+#include <ceres/version.h>
+
+#define CERES_VERSION_LT_2_1 (CERES_VERSION_MAJOR < 2 || (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR < 1))
+#if CERES_VERSION_LT_2_1
 #include <ceres/local_parameterization.h>
+#else
+#include <ceres/manifold.h>
+#endif
 #include <Eigen/Core>
 
 // Ceres Solver - A fast non-linear least squares minimizer
@@ -127,8 +134,13 @@ void addSubsetParameterization(ceres::Problem& problem, const std::map<const dou
         }
         else
         {
+#if CERES_VERSION_LT_2_1
           ceres::LocalParameterization* lp = new ceres::SubsetParameterization(block_size, mask);
           problem.SetParameterization(p, lp);
+#else
+          ceres::Manifold* m = new ceres::SubsetManifold(block_size, mask);
+          problem.SetManifold(p, m);
+#endif
         }
       }
     }
diff --git a/test/local_parameterization_utest.cpp b/test/local_parameterization_utest.cpp
index 4d8b349f..6e720d42 100644
--- a/test/local_parameterization_utest.cpp
+++ b/test/local_parameterization_utest.cpp
@@ -40,7 +40,11 @@ TEST(LocalParameterizationTests, SubsetParameterization)
     EXPECT_NO_THROW(addSubsetParameterization(problem, mask));
 
     // Expect there to be no parameterization, but the entire block should be constant
+#if CERES_VERSION_LT_2_1
     EXPECT_EQ(problem.GetParameterization(params.data()), nullptr);
+#else
+    EXPECT_EQ(problem.GetManifold(params.data()), nullptr);
+#endif
     EXPECT_TRUE(problem.IsParameterBlockConstant(params.data()));
   }
 
@@ -51,7 +55,11 @@ TEST(LocalParameterizationTests, SubsetParameterization)
     int bad_idx = params.size() * 2;
     mask[params.data()].insert(mask[params.data()].begin(), { bad_idx, 0, 1, 2 });
     EXPECT_THROW(addSubsetParameterization(problem, mask), OptimizationException);
+#if CERES_VERSION_LT_2_1
     EXPECT_EQ(problem.GetParameterization(params.data()), nullptr);
+#else
+    EXPECT_EQ(problem.GetManifold(params.data()), nullptr);
+#endif
   }
 
   // Empty mask
@@ -59,7 +67,11 @@ TEST(LocalParameterizationTests, SubsetParameterization)
     std::map<const double*, std::vector<int>> mask;
     EXPECT_NO_THROW(addSubsetParameterization(problem, mask));
     // An empty mask should not have added any local parameterization
+#if CERES_VERSION_LT_2_1
     EXPECT_EQ(problem.GetParameterization(params.data()), nullptr);
+#else
+    EXPECT_EQ(problem.GetManifold(params.data()), nullptr);
+#endif
   }
 
   // Hold the zero-th row constant
@@ -73,7 +85,11 @@ TEST(LocalParameterizationTests, SubsetParameterization)
       mask[params.data()].push_back(i * params.rows());
     }
     EXPECT_NO_THROW(addSubsetParameterization(problem, mask));
+#if CERES_VERSION_LT_2_1
     EXPECT_NE(problem.GetParameterization(params.data()), nullptr);
+#else
+    EXPECT_NE(problem.GetManifold(params.data()), nullptr);
+#endif
   }
 
   // Solve the optimization