some changes that get testPriorFactor compiling

gtsam/navigation/CombinedImuFactor.cpp

@@ -222,9 +222,9 @@ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
 Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
     const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
     const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
+    OptionalMatrixType H1, OptionalMatrixType H2,
-    boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
+    OptionalMatrixType H3, OptionalMatrixType H4,
-    boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
+    OptionalMatrixType H5, OptionalMatrixType H6) const {
 
   // error wrt bias evolution model (random walk)
   Matrix6 Hbias_i, Hbias_j;

gtsam/navigation/GPSFactor.cpp

@@ -80,7 +80,7 @@ bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
 
 //***************************************************************************
 Vector GPSFactor2::evaluateError(const NavState& p,
-    boost::optional<Matrix&> H) const {
+    OptionalMatrixType H) const {
   return p.position(H) -nT_;
 }
 

gtsam/navigation/GPSFactor.h

@@ -152,7 +152,7 @@ public:
 
   /// vector of errors
   Vector evaluateError(const NavState& p,
-      boost::optional<Matrix&> H = boost::none) const override;
+      OptionalMatrixType H = OptionalNone) const override;
 
   inline const Point3 & measurementIn() const {
     return nT_;

gtsam/nonlinear/CustomFactor.cpp

@@ -43,7 +43,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) c
        *    return error
        * ```
        */
-      return this->error_function_(*this, x, H.get_ptr());
+      return this->error_function_(*this, x, &(*H));
     } else {
       /*
        * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python.

gtsam/nonlinear/CustomFactor.h

@@ -75,7 +75,7 @@ public:
     * Calls the errorFunction closure, which is a std::function object
     * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array
     */
-  Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = boost::none) const override;
+  Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override;
 
   /** print */
   void print(const std::string &s,

gtsam/nonlinear/ExpressionFactor.h

@@ -97,7 +97,7 @@ protected:
    * both the function evaluation and its derivative(s) in H.
    */
   Vector unwhitenedError(const Values& x,
-    OptionalMatrixVecType H = OptionalNone) const override {
+    OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
     if (H) {
       const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
       // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here

gtsam/nonlinear/NonlinearFactor.h

@@ -41,13 +41,17 @@ namespace gtsam {
 // These typedefs and aliases will help with making the evaluateError interface
 // independent of boost
 using OptionalNoneType = std::nullptr_t;
-#define OptionalNone = nullptr
+// TODO: Change this to OptionalMatrixNone
+#define OptionalNone static_cast<Matrix*>(nullptr)
 template <typename T = void>
 using OptionalMatrixTypeT = Matrix*;
+template <typename T = void>
+using MatrixTypeT = Matrix;
 using OptionalMatrixType = Matrix*;
 // These typedefs and aliases will help with making the unwhitenedError interface
 // independent of boost
 using OptionalMatrixVecType = std::vector<Matrix>*;
+#define OptionalMatrixVecNone static_cast<std::vector<Matrix>*>(nullptr)
 #else
 // creating a none value to use when declaring our interfaces
 using OptionalNoneType = boost::none_t;
@@ -56,6 +60,7 @@ template <typename T = void>
 using OptionalMatrixTypeT = boost::optional<Matrix&>;
 using OptionalMatrixType = boost::optional<Matrix&>;
 using OptionalMatrixVecType = boost::optional<std::vector<Matrix>&>;
+#define OptionalMatrixVecNone boost::none
 #endif
 /**
  * Nonlinear factor base class
@@ -252,10 +257,10 @@ public:
    * If the optional arguments is specified, it should compute
    * both the function evaluation and its derivative(s) in H.
    */
-  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const = 0;
+  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const = 0;
-#ifdef NO_BOOST_C17
+#ifdef NO_BOOST_CPP17
   // support taking in the actual vector instead of the pointer as well
-  Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) {
+  Vector unwhitenedError(const Values& x, std::vector<Matrix>& H) const {
 	  return unwhitenedError(x, &H);
   }
 #endif
@@ -565,7 +570,7 @@ protected:
    */
   Vector unwhitenedError(
       const Values& x,
-      OptionalMatrixVecType H = OptionalNone) const override {
+      OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
     return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
                            H);
   }
@@ -599,8 +604,15 @@ protected:
   virtual Vector evaluateError(const ValueTypes&... x,
                                OptionalMatrixTypeT<ValueTypes>... H) const = 0;
 
-  /// @}
+#ifdef NO_BOOST_CPP17
+  // if someone uses the evaluateError function by supplying all the optional
+  // arguments then redirect the call to the one which takes pointers
+  Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
+	  return evaluateError(x..., (&H)...);
+  }
+#endif
 
+  /// @}
   /// @name Convenience method overloads
   /// @{
 
@@ -630,7 +642,7 @@ protected:
                   "or Matrix*");
 	// if they pass all matrices then we want to pass their pointers instead
 	if constexpr (are_all_mat) {
-		return evaluateError(x..., (&OptionalJacArgs)...);
+		return evaluateError(x..., (&H)...);
 	} else {
     return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
 	}
@@ -659,7 +671,7 @@ protected:
   inline Vector unwhitenedError(
       boost::mp11::index_sequence<Indices...>,  //
       const Values& x,
-      OptionalMatrixVecType H = OptionalNone) const {
+      OptionalMatrixVecType H = OptionalMatrixVecNone) const {
     if (this->active(x)) {
       if (H) {
         return evaluateError(x.at<ValueTypes>(keys_[Indices])...,

gtsam/sfm/ShonanFactor.cpp

@@ -75,8 +75,8 @@ bool ShonanFactor<d>::equals(const NonlinearFactor &expected,
 //******************************************************************************
 template <size_t d>
 void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
-                                    boost::optional<Matrix &> H1,
+                                    OptionalMatrixType H1,
-                                    boost::optional<Matrix &> H2) const {
+                                    OptionalMatrixType H2) const {
   gttic(ShonanFactor_Jacobians);
   const size_t dim = p_ * d; // Stiefel manifold dimension
 
@@ -106,8 +106,8 @@ void ShonanFactor<d>::fillJacobians(const Matrix &M1, const Matrix &M2,
 //******************************************************************************
 template <size_t d>
 Vector ShonanFactor<d>::evaluateError(const SOn &Q1, const SOn &Q2,
-                                      boost::optional<Matrix &> H1,
+                                      OptionalMatrixType H1,
-                                      boost::optional<Matrix &> H2) const {
+                                      OptionalMatrixType H2) const {
   gttic(ShonanFactor_evaluateError);
 
   const Matrix &M1 = Q1.matrix();

gtsam/sfm/ShonanFactor.h

@@ -73,15 +73,15 @@ public:
   /// projects down from SO(p) to the Stiefel manifold of px3 matrices.
   Vector
   evaluateError(const SOn &Q1, const SOn &Q2,
-                boost::optional<Matrix &> H1 = boost::none,
+                OptionalMatrixType H1 = OptionalNone,
-                boost::optional<Matrix &> H2 = boost::none) const override;
+                OptionalMatrixType H2 = OptionalNone) const override;
   /// @}
 
 private:
   /// Calculate Jacobians if asked, Only implemented for d=2 and 3 in .cpp
   void fillJacobians(const Matrix &M1, const Matrix &M2,
-                     boost::optional<Matrix &> H1,
+                     OptionalMatrixType H1,
-                     boost::optional<Matrix &> H2) const;
+                     OptionalMatrixType H2) const;
 };
 
 // Explicit instantiation for d=2 and d=3 in .cpp file:

gtsam/slam/OrientedPlane3Factor.h

@@ -44,8 +44,8 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
   /// evaluateError
   Vector evaluateError(
       const Pose3& pose, const OrientedPlane3& plane,
-      boost::optional<Matrix&> H1 = boost::none,
+      OptionalMatrixType H1 = OptionalNone,
-      boost::optional<Matrix&> H2 = boost::none) const override;
+      OptionalMatrixType H2 = OptionalNone) const override;
 };
 
 // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
@@ -73,7 +73,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   Vector evaluateError(const OrientedPlane3& plane,
-      boost::optional<Matrix&> H = boost::none) const override;
+      OptionalMatrixType H = OptionalNone) const override;
 };
 
 } // gtsam

gtsam_unstable/slam/MultiProjectionFactor.h

@@ -129,7 +129,7 @@ namespace gtsam {
     }
 
     /// Evaluate error h(x)-z and optionally derivatives
-    Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalNone) const override {
+    Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
 
       Vector a;
       return a;