From 4eb73db1c1faf43085bdca659fb3d7a6336d67bc Mon Sep 17 00:00:00 2001
From: kartik arcot <kartik.arcot@verdantrobotics.com>
Date: Wed, 4 Jan 2023 15:58:51 -0800
Subject: [PATCH] breaks

---
 CMakeLists.txt                    |  2 +-
 gtsam/nonlinear/NonlinearFactor.h | 23 ++++++++++++++++-------
 2 files changed, 17 insertions(+), 8 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 994f295e3..fc9cd2e36 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,9 +9,9 @@ endif()
 option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON)
 add_definitions(-Wno-deprecated-declarations)
 
+  set(CMAKE_CXX_STANDARD 17)
 if (GTSAM_NO_BOOST_CPP17)
   add_definitions(-DNO_BOOST_CPP17)
-  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 # Set the version number for the library
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index acaa24424..235858088 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -251,7 +251,12 @@ public:
    * both the function evaluation and its derivative(s) in H.
    */
   virtual Vector unwhitenedError(const Values& x, OptionalMatrixVec H = OptionalNone) const = 0;
-
+#ifdef NO_BOOST_C17
+  // support taking in the actual vector instead of the pointer as well
+  Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) {
+	  return unwhitenedError(x, &H);
+  }
+#endif
   /**
    * Vector of errors, whitened
    * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
@@ -611,12 +616,16 @@ protected:
    *
    * e.g. `const Vector error = factor.evaluateError(pose, point, Hpose);`
    */
-  template <typename... OptionalJacArgs,
-            typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
-  inline Vector evaluateError(const ValueTypes&... x,
-                              OptionalJacArgs&&... H) const {
-    return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
-                         OptionalNone);
+  template <typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
+  inline Vector evaluateError(const ValueTypes&... x, OptionalJacArgs&&... H) const {
+	constexpr bool are_all_opt_mat = (... && std::is_same<OptionalMatrix, std::decay_t<OptionalJacArgs>>::value);
+	constexpr bool are_all_mat = (... && std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value);
+	static_assert((are_all_mat == false && are_all_opt_mat == false), "ERRORRR");
+    if constexpr ((... && std::is_same<OptionalMatrix, std::decay_t<OptionalJacArgs>>::value)) {
+      return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., OptionalNone);
+    } else if constexpr ((... && std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value)) {
+      return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., OptionalNone);
+    }
   }
 
   /// @}