Add plus/minus functions

daoran · Jun 20, 2024 · bfaa11b · bfaa11b
1 parent 5145f39
commit bfaa11b
Showing 1 changed file with 17 additions and 4 deletions.
diff --git a/include/manif/ceres/manifold.h b/include/manif/ceres/manifold.h
@@ -25,10 +25,9 @@ class CeresManifoldFunctor
   CeresManifoldFunctor() = default;
   virtual ~CeresManifoldFunctor() = default;
 
-  template<typename T>
-  bool operator()(const T* state_raw,
-                  const T* delta_raw,
-                  T* state_plus_delta_raw) const
+  bool Plus(const double* state_raw,
+            const double* delta_raw,
+            double* state_plus_delta_raw) const
   {
     const Eigen::Map<const LieGroupTemplate<T>> state(state_raw);
     const Eigen::Map<const TangentTemplate<T>>  delta(delta_raw);
@@ -39,6 +38,20 @@ class CeresManifoldFunctor
 
     return true;
   }
+
+  bool Minus(const double* y_raw,
+             const double* x_raw,
+             double* y_minus_x_raw) const
+  {
+    const Eigen::Map<const LieGroupTemplate<T>> y(y_raw);
+    const Eigen::Map<const TangentTemplate<T>>  x(x_raw);
+
+    Eigen::Map<LieGroupTemplate<T>> y_minus_x(y_minus_x_raw);
+
+    y_minus_x = y - x;
+
+    return true;
+  }
 };
 
 } /* namespace manif */