From ce0287314091f1e9f7bca07b16624d24742cb815 Mon Sep 17 00:00:00 2001
From: kartik arcot <kartik.arcot@verdantrobotics.com>
Date: Fri, 6 Jan 2023 16:29:15 -0800
Subject: [PATCH] everything compiles but tests fail in no boost mode

---
 doc/Code/LocalizationFactor.cpp               |   4 +-
 examples/LocalizationExample.cpp              |   3 +-
 gtsam/navigation/AHRSFactor.h                 |   6 +-
 gtsam/navigation/AttitudeFactor.h             |   6 +-
 gtsam/navigation/CombinedImuFactor.h          |  10 +-
 gtsam/navigation/GPSFactor.h                  |   8 +-
 gtsam/navigation/ImuFactor.h                  |  11 +-
 gtsam/navigation/MagFactor.h                  |  18 +--
 gtsam/navigation/MagPoseFactor.h              |   4 +-
 gtsam/navigation/tests/testAttitudeFactor.cpp |  19 +--
 .../navigation/tests/testBarometricFactor.cpp |  18 +--
 gtsam/navigation/tests/testMagPoseFactor.cpp  |  10 +-
 gtsam/nonlinear/ExpressionFactor.h            | 114 ++++++++++++++++--
 gtsam/nonlinear/FunctorizedFactor.h           |   2 +
 gtsam/nonlinear/NonlinearEquality.h           |  11 +-
 gtsam/nonlinear/NonlinearFactor.h             |   2 +
 gtsam/nonlinear/PriorFactor.h                 |   3 +-
 .../nonlinear/tests/testFunctorizedFactor.cpp |   5 +-
 gtsam/sam/BearingFactor.h                     |  10 +-
 gtsam/sam/BearingRangeFactor.h                |   8 +-
 gtsam/sam/RangeFactor.h                       |  25 ++--
 gtsam/slam/BetweenFactor.h                    |   5 +-
 gtsam/slam/BoundingConstraint.h               |  10 +-
 gtsam/slam/EssentialMatrixConstraint.h        |   4 +-
 gtsam/slam/EssentialMatrixFactor.h            |   8 ++
 gtsam/slam/FrobeniusFactor.h                  |   7 +-
 gtsam/slam/OrientedPlane3Factor.h             |   8 +-
 gtsam/slam/PoseRotationPrior.h                |   4 +-
 gtsam/slam/PoseTranslationPrior.h             |   4 +-
 gtsam/slam/ProjectionFactor.h                 |   4 +-
 gtsam/slam/ReferenceFrameFactor.h             |   4 +-
 gtsam/slam/RotateFactor.h                     |   7 +-
 gtsam/slam/StereoFactor.h                     |   5 +-
 gtsam/slam/TriangulationFactor.h              |   4 +-
 gtsam/slam/tests/testBetweenFactor.cpp        |   9 +-
 .../tests/testEssentialMatrixConstraint.cpp   |   9 +-
 gtsam/slam/tests/testOrientedPlane3Factor.cpp |  15 +--
 gtsam/slam/tests/testRotateFactor.cpp         |  14 +--
 gtsam/slam/tests/testTriangulationFactor.cpp  |   6 +-
 tests/simulated2D.h                           |  26 ++--
 tests/simulated2DOriented.h                   |  11 +-
 tests/simulated3D.h                           |  24 ++--
 tests/smallExample.h                          |   3 +-
 tests/testExtendedKalmanFilter.cpp            |   8 +-
 tests/testNonlinearFactor.cpp                 |  34 +++---
 45 files changed, 315 insertions(+), 215 deletions(-)

diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp
index 74be3b924..100f6fa2e 100644
--- a/doc/Code/LocalizationFactor.cpp
+++ b/doc/Code/LocalizationFactor.cpp
@@ -2,11 +2,11 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   double mx_, my_; ///< X and Y measurements
   
 public:
+  using gtsam::NoiseModelFactor1<Pose2>::evaluateError;
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
-  Vector evaluateError(const Pose2& q, 
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
     const Rot2& R = q.rotation();
     if (H) (*H) = (gtsam::Matrix(2, 3) <<
             R.c(), -R.s(), 0.0,
diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp
index a8f2b22c6..79258b287 100644
--- a/examples/LocalizationExample.cpp
+++ b/examples/LocalizationExample.cpp
@@ -71,6 +71,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
   double mx_, my_;
 
  public:
+  using NoiseModelFactor1<Pose2>::evaluateError;
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<UnaryFactor> shared_ptr;
 
@@ -84,7 +85,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
-  Vector evaluateError(const Pose2& q, OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
     // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
     // The error is then simply calculated as E(q) = h(q) - m:
     // error_x = q.x - mx
diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h
index 2a3c98c37..e5a92ec47 100644
--- a/gtsam/navigation/AHRSFactor.h
+++ b/gtsam/navigation/AHRSFactor.h
@@ -140,6 +140,7 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
 
 public:
 
+  using Base::evaluateError;
   /** Shorthand for a smart pointer to a factor */
 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
   typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
@@ -179,9 +180,8 @@ public:
 
   /// vector of errors
   Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
-      const Vector3& bias, OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 =
-          OptionalNone) const override;
+      const Vector3& bias, OptionalMatrixType H1,
+      OptionalMatrixType H2, OptionalMatrixType H3) const override;
 
   /// predicted states from IMU
   /// TODO(frank): relationship with PIM predict ??
diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 37bc29164..4339fc5de 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -122,8 +122,7 @@ public:
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  Vector evaluateError(const Rot3& nRb, //
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
     return attitudeError(nRb, H);
   }
 
@@ -198,8 +197,7 @@ public:
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  Vector evaluateError(const Pose3& nTb, //
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override {
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;
diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h
index 91f05489e..fc12519bd 100644
--- a/gtsam/navigation/CombinedImuFactor.h
+++ b/gtsam/navigation/CombinedImuFactor.h
@@ -268,6 +268,7 @@ private:
   PreintegratedCombinedMeasurements _PIM_;
 
 public:
+  using Base::evaluateError;
 
   /** Shorthand for a smart pointer to a factor */
 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
@@ -324,12 +325,9 @@ public:
   Vector 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,
-      OptionalMatrixType H1 = OptionalNone, 
-      OptionalMatrixType H2 = OptionalNone, 
-      OptionalMatrixType H3 = OptionalNone,
-      OptionalMatrixType H4 = OptionalNone, 
-      OptionalMatrixType H5 = OptionalNone, 
-      OptionalMatrixType H6 = OptionalNone) const override;
+      OptionalMatrixType H1, OptionalMatrixType H2, 
+      OptionalMatrixType H3, OptionalMatrixType H4, 
+      OptionalMatrixType H5, OptionalMatrixType H6) const override;
 
  private:
   /** Serialization function */
diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index 73ec7d5a7..df2b2474f 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -42,6 +42,7 @@ private:
 
 public:
 
+  using Base::evaluateError;
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<GPSFactor> shared_ptr;
 
@@ -78,8 +79,7 @@ public:
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /// vector of errors
-  Vector evaluateError(const Pose3& p,
-      OptionalMatrixType H = OptionalNone) const override;
+  Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
 
   inline const Point3 & measurementIn() const {
     return nT_;
@@ -120,6 +120,7 @@ private:
   Point3 nT_; ///< Position measurement in cartesian coordinates
 
 public:
+  using Base::evaluateError;
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<GPSFactor2> shared_ptr;
@@ -151,8 +152,7 @@ public:
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /// vector of errors
-  Vector evaluateError(const NavState& p,
-      OptionalMatrixType H = OptionalNone) const override;
+  Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
 
   inline const Point3 & measurementIn() const {
     return nT_;
diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h
index 51b86a00f..e521e6b19 100644
--- a/gtsam/navigation/ImuFactor.h
+++ b/gtsam/navigation/ImuFactor.h
@@ -229,10 +229,8 @@ public:
   /// vector of errors
   Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
       const Pose3& pose_j, const Vector3& vel_j,
-      const imuBias::ConstantBias& bias_i, OptionalMatrixType H1 =
-          OptionalNone, OptionalMatrixType H2 = OptionalNone,
-      OptionalMatrixType H3 = OptionalNone, OptionalMatrixType H4 =
-          OptionalNone, OptionalMatrixType H5 = OptionalNone) const override;
+      const imuBias::ConstantBias& bias_i, OptionalMatrixType H1, OptionalMatrixType H2,
+      OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) const override;
 
 #ifdef GTSAM_TANGENT_PREINTEGRATION
   /// Merge two pre-integrated measurement classes
@@ -308,9 +306,8 @@ public:
   /// vector of errors
   Vector evaluateError(const NavState& state_i, const NavState& state_j,
                        const imuBias::ConstantBias& bias_i,  //
-                       OptionalMatrixType H1 = OptionalNone,
-                       OptionalMatrixType H2 = OptionalNone,
-                       OptionalMatrixType H3 = OptionalNone) const override;
+                       OptionalMatrixType H1, OptionalMatrixType H2,
+                       OptionalMatrixType H3) const override;
 
 private:
 
diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h
index 95523e18f..d57a5a72d 100644
--- a/gtsam/navigation/MagFactor.h
+++ b/gtsam/navigation/MagFactor.h
@@ -37,6 +37,7 @@ class MagFactor: public NoiseModelFactorN<Rot2> {
   const Point3 bias_; ///< bias
 
 public:
+  using NoiseModelFactor1<Rot2>::evaluateError;
 
   /**
    * Constructor of factor that estimates nav to body rotation bRn
@@ -74,8 +75,7 @@ public:
   /**
    * @brief vector of errors
    */
-  Vector evaluateError(const Rot2& nRb,
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Rot2& nRb, OptionalMatrixType H) const override {
     // measured bM = nRb� * nM + b
     Point3 hx = unrotate(nRb, nM_, H) + bias_;
     return (hx - measured_);
@@ -94,6 +94,7 @@ class MagFactor1: public NoiseModelFactorN<Rot3> {
   const Point3 bias_; ///< bias
 
 public:
+  using NoiseModelFactor1<Rot3>::evaluateError;
 
   /** Constructor */
   MagFactor1(Key key, const Point3& measured, double scale,
@@ -112,8 +113,7 @@ public:
   /**
    * @brief vector of errors
    */
-  Vector evaluateError(const Rot3& nRb,
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
     // measured bM = nRb� * nM + b
     Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
     return (hx - measured_);
@@ -131,6 +131,7 @@ class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
   const Rot3 bRn_; ///< The assumed known rotation from nav to body
 
 public:
+  using NoiseModelFactor2<Point3, Point3>::evaluateError;
 
   /** Constructor */
   MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
@@ -151,8 +152,7 @@ public:
    * @param bias (unknown) 3D bias
    */
   Vector evaluateError(const Point3& nM, const Point3& bias,
-      OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 =
-          OptionalNone) const override {
+      OptionalMatrixType H1, OptionalMatrixType H2) const override {
     // measured bM = nRb� * nM + b, where b is unknown bias
     Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias;
     if (H2)
@@ -172,6 +172,7 @@ class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
   const Rot3 bRn_; ///< The assumed known rotation from nav to body
 
 public:
+  using NoiseModelFactor3<double, Unit3, Point3>::evaluateError;
 
   /** Constructor */
   MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
@@ -192,9 +193,8 @@ public:
    * @param bias (unknown) 3D bias
    */
   Vector evaluateError(const double& scale, const Unit3& direction,
-      const Point3& bias, OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone, OptionalMatrixType H3 =
-          OptionalNone) const override {
+      const Point3& bias, OptionalMatrixType H1,
+      OptionalMatrixType H2, OptionalMatrixType H3) const override {
     // measured bM = nRb� * nM + b, where b is unknown bias
     Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2);
     Point3 hx = scale * rotated.point3() + bias;
diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h
index d5350384c..bf64933dc 100644
--- a/gtsam/navigation/MagPoseFactor.h
+++ b/gtsam/navigation/MagPoseFactor.h
@@ -49,6 +49,8 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
   GTSAM_CONCEPT_POSE_TYPE(POSE)
 
  public:
+  using Base::evaluateError;
+
   ~MagPoseFactor() override {}
 
   /// Default constructor - only use for serialization.
@@ -108,7 +110,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
    * Return the factor's error h(x) - z, and the optional Jacobian. Note that
    * the measurement error is expressed in the body frame.
    */
-  Vector evaluateError(const POSE& nPb, OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const POSE& nPb, OptionalMatrixType H) const override {
     // Predict the measured magnetic field h(x) in the *body* frame.
     // If body_P_sensor was given, bias_ will have been rotated into the body frame.
     Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index 26d793528..3bd07a323 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -21,6 +21,8 @@
 #include <gtsam/base/numericalDerivative.h>
 
 #include <boost/bind/bind.hpp>
+#include <functional>
+#include "gtsam/base/Matrix.h"
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std::placeholders;
@@ -47,11 +49,11 @@ TEST( Rot3AttitudeFactor, Constructor ) {
   Rot3 nRb;
   EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5));
 
+  std::function<Vector(const Rot3&)> err_fn = [&factor](const Rot3& r){
+	return factor.evaluateError(r, OptionalNone);
+  };
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector, Rot3>(
-      std::bind(&Rot3AttitudeFactor::evaluateError, &factor,
-                std::placeholders::_1, boost::none),
-      nRb);
+  Matrix expectedH = numericalDerivative11<Vector, Rot3>(err_fn, nRb);
 
   // Use the factor to calculate the derivative
   Matrix actualH;
@@ -98,10 +100,13 @@ TEST( Pose3AttitudeFactor, Constructor ) {
   Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));
   EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5));
 
+  Matrix actualH1;
+
+  std::function<Vector(const Pose3&)> err_fn = [&factor](const Pose3& p){
+	return factor.evaluateError(p, OptionalNone);
+  };
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,Pose3>(
-      std::bind(&Pose3AttitudeFactor::evaluateError, &factor, std::placeholders::_1,
-          boost::none), T);
+  Matrix expectedH = numericalDerivative11<Vector,Pose3>(err_fn, T);
 
   // Use the factor to calculate the derivative
   Matrix actualH;
diff --git a/gtsam/navigation/tests/testBarometricFactor.cpp b/gtsam/navigation/tests/testBarometricFactor.cpp
index 47f4824c1..85ca25075 100644
--- a/gtsam/navigation/tests/testBarometricFactor.cpp
+++ b/gtsam/navigation/tests/testBarometricFactor.cpp
@@ -58,15 +58,11 @@ TEST(BarometricFactor, Constructor) {
 
     // Calculate numerical derivatives
     Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
-        std::bind(&BarometricFactor::evaluateError, &factor,
-                  std::placeholders::_1, std::placeholders::_2, boost::none,
-                  boost::none),
-        T, baroBias);
+        [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
+		T, baroBias);
 
     Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
-        std::bind(&BarometricFactor::evaluateError, &factor,
-                  std::placeholders::_1, std::placeholders::_2, boost::none,
-                  boost::none),
+        [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
         T, baroBias);
 
     // Use the factor to calculate the derivative
@@ -99,15 +95,11 @@ TEST(BarometricFactor, nonZero) {
 
     // Calculate numerical derivatives
     Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
-        std::bind(&BarometricFactor::evaluateError, &factor,
-                  std::placeholders::_1, std::placeholders::_2, boost::none,
-                  boost::none),
+        [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
         T, baroBias);
 
     Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
-        std::bind(&BarometricFactor::evaluateError, &factor,
-                  std::placeholders::_1, std::placeholders::_2, boost::none,
-                  boost::none),
+        [&factor](const Pose3& p, const double& d) {return factor.evaluateError(p, d);},
         T, baroBias);
 
     // Use the factor to calculate the derivative and the error
diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp
index e10409f4c..9fb81cd7f 100644
--- a/gtsam/navigation/tests/testMagPoseFactor.cpp
+++ b/gtsam/navigation/tests/testMagPoseFactor.cpp
@@ -78,8 +78,7 @@ TEST(MagPoseFactor, JacobianPose2) {
   MagPoseFactor<Pose2> f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none);
   CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2>  //
-                            (std::bind(&MagPoseFactor<Pose2>::evaluateError, &f,
-                                       std::placeholders::_1, boost::none),
+                            ([&f] (const Pose2& p) {return f.evaluateError(p);},
                              n_P2_b),
                             H2, 1e-7));
 }
@@ -92,8 +91,7 @@ TEST(MagPoseFactor, JacobianPose3) {
   MagPoseFactor<Pose3> f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none);
   CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3>  //
-                            (std::bind(&MagPoseFactor<Pose3>::evaluateError, &f,
-                                       std::placeholders::_1, boost::none),
+                            ([&f] (const Pose3& p) {return f.evaluateError(p);},
                              n_P3_b),
                             H3, 1e-7));
 }
@@ -109,7 +107,7 @@ TEST(MagPoseFactor, body_P_sensor2) {
   MagPoseFactor<Pose2> f = MagPoseFactor<Pose2>(Symbol('X', 0), sM, scale, dir2, bias2, model2, body_P2_sensor);
   CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose2> //
-      (std::bind(&MagPoseFactor<Pose2>::evaluateError, &f, std::placeholders::_1, boost::none), n_P2_b), H2, 1e-7));
+      ([&f] (const Pose2& p) {return f.evaluateError(p);},n_P2_b), H2, 1e-7));
 }
 
 // *****************************************************************************
@@ -123,7 +121,7 @@ TEST(MagPoseFactor, body_P_sensor3) {
   MagPoseFactor<Pose3> f = MagPoseFactor<Pose3>(Symbol('X', 0), sM, scale, dir3, bias3, model3, body_P3_sensor);
   CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5));
   CHECK(gtsam::assert_equal(gtsam::numericalDerivative11<Vector, Pose3> //
-      (std::bind(&MagPoseFactor<Pose3>::evaluateError, &f, std::placeholders::_1, boost::none), n_P3_b), H3, 1e-7));
+      ([&f] (const Pose3& p) {return f.evaluateError(p);}, n_P3_b), H3, 1e-7));
 }
 
 // *****************************************************************************
diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h
index 77add0012..3427e6079 100644
--- a/gtsam/nonlinear/ExpressionFactor.h
+++ b/gtsam/nonlinear/ExpressionFactor.h
@@ -28,6 +28,97 @@
 
 namespace gtsam {
 
+template <typename... ValueTypes>
+class EvaluateErrorInterface {
+public:
+  enum { N = sizeof...(ValueTypes) };
+
+private:
+  template <int I>
+  using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N),
+                                               void>::type;  // 1-indexed!
+
+public:
+  /**
+   * Override `evaluateError` to finish implementing an n-way factor.
+   *
+   * Both the `x` and `H` arguments are written here as parameter packs, but
+   * when overriding this method, you probably want to explicitly write them
+   * out.  For example, for a 2-way factor with variable types Pose3 and Point3,
+   * you should implement:
+   * ```
+   * Vector evaluateError(
+   *     const Pose3& x1, const Point3& x2,
+   *     boost::optional<Matrix&> H1 = boost::none,
+   *     boost::optional<Matrix&> H2 = boost::none) const override { ... }
+   * ```
+   *
+   * If any of the optional Matrix reference arguments are specified, it should
+   * compute both the function evaluation and its derivative(s) in the requested
+   * variables.
+   *
+   * @param x The values of the variables to evaluate the error for.  Passed in
+   * as separate arguments.
+   * @param[out] H The Jacobian with respect to each variable (optional).
+   */
+  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
+  /// @{
+
+  /** No-Jacobians requested function overload.
+   * This specializes the version below to avoid recursive calls since this is
+   * commonly used.
+   *
+   * e.g. `const Vector error = factor.evaluateError(pose, point);`
+   */
+  inline Vector evaluateError(const ValueTypes&... x) const {
+    return evaluateError(x..., OptionalMatrixTypeT<ValueTypes>()...);
+  }
+
+  /** Some (but not all) optional Jacobians are omitted (function overload)
+   *
+   * 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 {
+#ifdef NO_BOOST_CPP17
+    // A check to ensure all arguments passed are all either matrices or are all pointers to matrices
+    constexpr bool are_all_mat = (... && (std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value));
+    constexpr bool are_all_ptrs = (... && (std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
+                                           std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
+    static_assert((are_all_mat || are_all_ptrs),
+                  "Arguments that are passed to the evaluateError function can only be of following the types: Matrix, "
+                  "or Matrix*");
+    // if they pass all matrices then we want to pass their pointers instead
+    if constexpr (are_all_mat) {
+      return evaluateError(x..., (&H)...);
+    } else {
+      return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., static_cast<OptionalMatrixType>(OptionalNone));
+    }
+#else
+    // A check to ensure all arguments passed are all either matrices or are optionals of matrix references
+    constexpr bool are_all_mat = (... && (std::is_same<Matrix&, OptionalJacArgs>::value ||
+                                          std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
+                                          std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
+    static_assert(
+        are_all_mat,
+        "Arguments that are passed to the evaluateError function can only be of following the types: Matrix&, "
+        "boost::optional<Matrix&>, or boost::none_t");
+    return evaluateError(x..., std::forward<OptionalJacArgs>(H)..., OptionalNone);
+#endif
+  }
+};
+
 /**
  * Factor that supports arbitrary expressions via AD.
  *
@@ -40,8 +131,8 @@ namespace gtsam {
  * \tparam T Type for measurements.
  *
  */
-template<typename T>
-class ExpressionFactor: public NoiseModelFactor {
+template <typename T>
+class ExpressionFactor : public NoiseModelFactor {
   BOOST_CONCEPT_ASSERT((IsTestable<T>));
 
 protected:
@@ -55,6 +146,7 @@ protected:
 
 
  public:
+  using NoiseModelFactor::unwhitenedError;
   typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
 
   /**
@@ -243,6 +335,7 @@ class ExpressionFactorN : public ExpressionFactor<T> {
 public:
   static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
   using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
+  using ExpressionFactor<T>::unwhitenedError;
 
   /// Destructor
   ~ExpressionFactorN() override = default;
@@ -305,15 +398,14 @@ struct traits<ExpressionFactorN<T, Args...>>
  * @deprecated Prefer the more general ExpressionFactorN<>.
  */
 template <typename T, typename A1, typename A2>
-class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
+class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2>, public EvaluateErrorInterface<A1, A2> {
 public:
   /// Destructor
   ~ExpressionFactor2() override {}
 
   /// Backwards compatible evaluateError, to make existing tests compile
-  Vector evaluateError(const A1 &a1, const A2 &a2,
-                       OptionalMatrixType H1 = OptionalNone,
-                       OptionalMatrixType H2 = OptionalNone) const {
+  virtual Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1,
+                               OptionalMatrixType H2) const override {
     Values values;
     values.insert(this->keys_[0], a1);
     values.insert(this->keys_[1], a2);
@@ -327,12 +419,9 @@ public:
   /// Recreate expression from given keys_ and measured_, used in load
   /// Needed to deserialize a derived factor
   virtual Expression<T> expression(Key key1, Key key2) const {
-    throw std::runtime_error(
-        "ExpressionFactor2::expression not provided: cannot deserialize.");
+    throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize.");
   }
-  Expression<T>
-  expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
-      const override {
+  Expression<T> expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys& keys) const override {
     return expression(keys[0], keys[1]);
   }
 
@@ -341,8 +430,7 @@ protected:
   ExpressionFactor2() {}
 
   /// Constructor takes care of keys, but still need to call initialize
-  ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
-                    const T &measurement)
+  ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel& noiseModel, const T& measurement)
       : ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
 };
 // ExpressionFactor2
diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h
index 4b117d2b4..53c92155b 100644
--- a/gtsam/nonlinear/FunctorizedFactor.h
+++ b/gtsam/nonlinear/FunctorizedFactor.h
@@ -65,6 +65,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
   std::function<R(T, OptionalMatrixType)> func_;  ///< functor instance
 
  public:
+  using Base::evaluateError;
   /** default constructor - only use for serialization */
   FunctorizedFactor() {}
 
@@ -165,6 +166,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
   FunctionType func_;  ///< functor instance
 
  public:
+  using Base::evaluateError;
   /** default constructor - only use for serialization */
   FunctorizedFactor2() {}
 
diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h
index db71556a1..1d3282dd9 100644
--- a/gtsam/nonlinear/NonlinearEquality.h
+++ b/gtsam/nonlinear/NonlinearEquality.h
@@ -139,8 +139,7 @@ public:
   }
 
   /// Error function
-  Vector evaluateError(const T& xj,
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const T& xj, OptionalMatrixType H) const override {
     const size_t nj = traits<T>::GetDimension(feasible_);
     if (allow_error_) {
       if (H)
@@ -209,6 +208,7 @@ class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
 
 public:
   typedef VALUE X;
+  using NoiseModelFactor1<VALUE>::evaluateError;
 
 protected:
   typedef NoiseModelFactorN<VALUE> Base;
@@ -249,8 +249,7 @@ public:
   }
 
   /// g(x) with optional derivative
-  Vector evaluateError(const X& x1,
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const X& x1, OptionalMatrixType H) const override {
     if (H)
       (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
     // manifold equivalent of h(x)-z -> log(z,h(x))
@@ -305,6 +304,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
 
  public:
   typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
+  using Base::evaluateError;
 
   /**
    * Constructor
@@ -325,8 +325,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
 
   /// g(x) with optional derivative2
   Vector evaluateError(
-      const T& x1, const T& x2, OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone) const override {
+      const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override {
     static const size_t p = traits<T>::dimension;
     if (H1) *H1 = -Matrix::Identity(p, p);
     if (H2) *H2 = Matrix::Identity(p, p);
diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 8bb965ef5..398bc833d 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -433,6 +433,8 @@ class NoiseModelFactorN
   /// N is the number of variables (N-way factor)
   enum { N = sizeof...(ValueTypes) };
 
+  using NoiseModelFactor::unwhitenedError;
+
 protected:
   using Base = NoiseModelFactor;
   using This = NoiseModelFactorN<ValueTypes...>;
diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h
index f132432f6..2c6161760 100644
--- a/gtsam/nonlinear/PriorFactor.h
+++ b/gtsam/nonlinear/PriorFactor.h
@@ -31,6 +31,7 @@ namespace gtsam {
 
   public:
     typedef VALUE T;
+	using NoiseModelFactor1<VALUE>::evaluateError;
 
   private:
 
@@ -91,7 +92,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const T& x, OptionalMatrixType H = OptionalNone) const override {
+    Vector evaluateError(const T& x, OptionalMatrixType H) const override {
       if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
       // manifold equivalent of z-x -> Local(x,z)
       return -traits<T>::Local(x, prior_);
diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
index 214c5efa7..dbc22c079 100644
--- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
+++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp
@@ -160,7 +160,7 @@ TEST(FunctorizedFactor, Functional) {
   Matrix X = Matrix::Identity(3, 3);
   Matrix measurement = multiplier * Matrix::Identity(3, 3);
 
-  std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
+  std::function<Matrix(Matrix, OptionalMatrixType)> functional =
       MultiplyFunctor(multiplier);
   auto factor =
       MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
@@ -233,8 +233,7 @@ TEST(FunctorizedFactor, Functional2) {
   Vector3 x(1, 2, 3);
   Vector measurement = A * x;
 
-  std::function<Matrix(Matrix, Matrix, boost::optional<Matrix &>,
-                       boost::optional<Matrix &>)>
+  std::function<Matrix(Matrix, Matrix, OptionalMatrixType, OptionalMatrixType)>
       functional = ProjectionFunctor();
   auto factor = MakeFunctorizedFactor2<Matrix, Vector>(keyA, keyx, measurement,
                                                        model2, functional);
diff --git a/gtsam/sam/BearingFactor.h b/gtsam/sam/BearingFactor.h
index cb39604f0..242c9b258 100644
--- a/gtsam/sam/BearingFactor.h
+++ b/gtsam/sam/BearingFactor.h
@@ -34,9 +34,10 @@ struct Bearing;
  */
 template <typename A1, typename A2,
           typename T = typename Bearing<A1, A2>::result_type>
-struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
+struct BearingFactor : public ExpressionFactorN<T, A1, A2>, public EvaluateErrorInterface<A1, A2>{
   typedef ExpressionFactorN<T, A1, A2> Base;
 
+  using EvaluateErrorInterface<A1,A2>::evaluateError;
   /// default constructor
   BearingFactor() {}
 
@@ -61,13 +62,12 @@ struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
     Base::print(s, kf);
   }
   
-  Vector evaluateError(const A1& a1, const A2& a2,
-    OptionalMatrixType H1 = OptionalNone,
-    OptionalMatrixType H2 = OptionalNone) const
+  virtual Vector evaluateError(const A1& a1, const A2& a2,
+    OptionalMatrixType H1, OptionalMatrixType H2) const override
   {
     std::vector<Matrix> Hs(2);
     const auto &keys = Factor::keys();
-    const Vector error = unwhitenedError(
+    const Vector error = this->unwhitenedError(
       {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
       Hs);
     if (H1) *H1 = Hs[0];
diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h
index f57fc8066..bdd0888fb 100644
--- a/gtsam/sam/BearingRangeFactor.h
+++ b/gtsam/sam/BearingRangeFactor.h
@@ -33,7 +33,7 @@ template <typename A1, typename A2,
           typename B = typename Bearing<A1, A2>::result_type,
           typename R = typename Range<A1, A2>::result_type>
 class BearingRangeFactor
-    : public ExpressionFactorN<BearingRange<A1, A2>, A1, A2> {
+    : public ExpressionFactorN<BearingRange<A1, A2>, A1, A2>, public EvaluateErrorInterface<A1, A2>{
  private:
   typedef BearingRange<A1, A2> T;
   typedef ExpressionFactorN<T, A1, A2> Base;
@@ -41,6 +41,7 @@ class BearingRangeFactor
 
  public:
   typedef boost::shared_ptr<This> shared_ptr;
+  using EvaluateErrorInterface<A1, A2>::evaluateError;
 
   /// Default constructor
   BearingRangeFactor() {}
@@ -73,9 +74,8 @@ class BearingRangeFactor
                          Expression<A2>(keys[1]));
   }
 
-  Vector evaluateError(const A1& a1, const A2& a2,
-      OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone) const
+  virtual Vector evaluateError(const A1& a1, const A2& a2,
+      OptionalMatrixType H1, OptionalMatrixType H2) const override
   {
     std::vector<Matrix> Hs(2);
     const auto &keys = Factor::keys();
diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h
index d96df39eb..d5fb93179 100644
--- a/gtsam/sam/RangeFactor.h
+++ b/gtsam/sam/RangeFactor.h
@@ -32,12 +32,13 @@ struct Range;
  * @ingroup sam
  */
 template <typename A1, typename A2 = A1, typename T = double>
-class RangeFactor : public ExpressionFactorN<T, A1, A2> {
+class RangeFactor : public ExpressionFactorN<T, A1, A2> , public EvaluateErrorInterface<A1, A2>{
  private:
   typedef RangeFactor<A1, A2> This;
   typedef ExpressionFactorN<T, A1, A2> Base;
 
  public:
+  using EvaluateErrorInterface<A1,A2>::evaluateError;
   /// default constructor
   RangeFactor() {}
 
@@ -58,16 +59,12 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
     Expression<A2> a2_(keys[1]);
     return Expression<T>(Range<A1, A2>(), a1_, a2_);
   }
-  
-  Vector evaluateError(const A1& a1, const A2& a2,
-      OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone) const
-  {
+
+  virtual Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1,
+                               OptionalMatrixType H2) const override {
     std::vector<Matrix> Hs(2);
-    const auto &keys = Factor::keys();
-    const Vector error = Base::unwhitenedError(
-      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
-      Hs);
+    const auto& keys = Factor::keys();
+    const Vector error = Base::unwhitenedError({{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs);
     if (H1) *H1 = Hs[0];
     if (H2) *H2 = Hs[1];
     return error;
@@ -100,7 +97,7 @@ struct traits<RangeFactor<A1, A2, T> >
  */
 template <typename A1, typename A2 = A1,
           typename T = typename Range<A1, A2>::result_type>
-class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
+class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> , public EvaluateErrorInterface<A1, A2>{
  private:
   typedef RangeFactorWithTransform<A1, A2> This;
   typedef ExpressionFactorN<T, A1, A2> Base;
@@ -108,6 +105,7 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
   A1 body_T_sensor_;  ///< The pose of the sensor in the body frame
 
  public:
+  using EvaluateErrorInterface<A1, A2>::evaluateError;
   //// Default constructor
   RangeFactorWithTransform() {}
 
@@ -136,9 +134,8 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
     return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
   }
 
-  Vector evaluateError(const A1& a1, const A2& a2,
-      OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone) const
+  virtual Vector evaluateError(const A1& a1, const A2& a2,
+      OptionalMatrixType H1, OptionalMatrixType H2) const override
   {
     std::vector<Matrix> Hs(2);
     const auto &keys = Factor::keys();
diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h
index 46f69f14b..a4ad013fc 100644
--- a/gtsam/slam/BetweenFactor.h
+++ b/gtsam/slam/BetweenFactor.h
@@ -55,6 +55,7 @@ namespace gtsam {
     VALUE measured_; /** The measurement */
 
   public:
+	using Base::evaluateError;
 
     // shorthand for a smart pointer to a factor
     typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
@@ -105,8 +106,8 @@ namespace gtsam {
     /// @{
 
     /// evaluate error, returns vector of errors size of tangent space
-    Vector evaluateError(const T& p1, const T& p2, OptionalMatrixType H1 =
-      OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
+    Vector evaluateError(const T& p1, const T& p2,
+			OptionalMatrixType H1, OptionalMatrixType H2) const override {
       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
       // manifold equivalent of h(x)-z -> log(z,h(x))
 #ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h
index dbc27f19c..4837d6c54 100644
--- a/gtsam/slam/BoundingConstraint.h
+++ b/gtsam/slam/BoundingConstraint.h
@@ -35,6 +35,8 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
   typedef NoiseModelFactorN<VALUE> Base;
   typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
 
+  using Base::evaluateError;
+
   double threshold_;
   bool isGreaterThan_; /// flag for greater/less than
 
@@ -64,8 +66,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
   }
 
-  Vector evaluateError(const X& x, OptionalMatrixType H =
-      OptionalNone) const override {
+  Vector evaluateError(const X& x, OptionalMatrixType H) const override {
     Matrix D;
     double error = value(x, D) - threshold_;
     if (H) {
@@ -105,6 +106,8 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
   typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
   typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
 
+  using Base::evaluateError;
+
   double threshold_;
   bool isGreaterThan_; /// flag for greater/less than
 
@@ -134,8 +137,7 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
   }
 
   Vector evaluateError(const X1& x1, const X2& x2,
-      OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone) const override {
+      OptionalMatrixType H1, OptionalMatrixType H2) const override {
     Matrix D1, D2;
     double error = value(x1, x2, D1, D2) - threshold_;
     if (H1) {
diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h
index 6d72d34af..a02dbb5f6 100644
--- a/gtsam/slam/EssentialMatrixConstraint.h
+++ b/gtsam/slam/EssentialMatrixConstraint.h
@@ -37,6 +37,7 @@ private:
   EssentialMatrix measuredE_; /** The measurement is an essential matrix */
 
 public:
+  using Base::evaluateError;
 
   // shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
@@ -79,8 +80,7 @@ public:
 
   /** vector of errors */
   Vector evaluateError(const Pose3& p1, const Pose3& p2,
-      OptionalMatrixType Hp1 = OptionalNone, //
-      OptionalMatrixType Hp2 = OptionalNone) const override;
+      OptionalMatrixType Hp1, OptionalMatrixType Hp2) const override;
 
   /** return the measured */
   const EssentialMatrix& measured() const {
diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h
index 6e5959df4..7f3875d06 100644
--- a/gtsam/slam/EssentialMatrixFactor.h
+++ b/gtsam/slam/EssentialMatrixFactor.h
@@ -295,7 +295,15 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
       // Version with derivatives
       Matrix D_e_cameraE, D_cameraE_E;  // 2*5, 5*5
       EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
+#ifdef NO_BOOST_CPP17
+	  // Had to do this since the only overloaded function EssentialMatrixFactor2
+	  // uses the type OptionalMatrixType. Which would be a pointer when we are
+	  // not using boost. There is no way to redirect that call to the top (NoiseModelFactorN)
+	  // dereference it and bring it back to the Base (EssentialMatrixFactor2)
+      Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd);
+#else
       Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
+#endif
       *DE = D_e_cameraE * D_cameraE_E;  // (2*5) * (5*5)
       return e;
     }
diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h
index 4bd1bf7d9..07c79b32d 100644
--- a/gtsam/slam/FrobeniusFactor.h
+++ b/gtsam/slam/FrobeniusFactor.h
@@ -79,6 +79,7 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
   enum { Dim = Rot::VectorN2::RowsAtCompileTime };
 
  public:
+  using NoiseModelFactor2<Rot, Rot>::evaluateError;
   /// Constructor
   FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
       : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
@@ -86,8 +87,7 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
 
   /// Error is just Frobenius norm between rotation matrices.
   Vector evaluateError(const Rot& R1, const Rot& R2,
-                       OptionalMatrixType H1 = OptionalNone,
-                       OptionalMatrixType H2 = OptionalNone) const override {
+                       OptionalMatrixType H1, OptionalMatrixType H2) const override {
     Vector error = R2.vec(H2) - R1.vec(H1);
     if (H1) *H1 = -*H1;
     return error;
@@ -151,8 +151,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
 
   /// Error is Frobenius norm between R1*R12 and R2.
   Vector evaluateError(const Rot& R1, const Rot& R2,
-                       OptionalMatrixType H1 = OptionalNone,
-                       OptionalMatrixType H2 = OptionalNone) const override {
+                       OptionalMatrixType H1, OptionalMatrixType H2) const override {
     const Rot R2hat = R1.compose(R12_);
     Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
     Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h
index 750215b72..a4d867e1c 100644
--- a/gtsam/slam/OrientedPlane3Factor.h
+++ b/gtsam/slam/OrientedPlane3Factor.h
@@ -21,6 +21,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
   typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
 
  public:
+  using NoiseModelFactor2<Pose3, OrientedPlane3>::evaluateError;
   /// Constructor
   OrientedPlane3Factor() {
   }
@@ -44,8 +45,7 @@ class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, Oriente
   /// evaluateError
   Vector evaluateError(
       const Pose3& pose, const OrientedPlane3& plane,
-      OptionalMatrixType H1 = OptionalNone,
-      OptionalMatrixType H2 = OptionalNone) const override;
+      OptionalMatrixType H1, OptionalMatrixType H2) const override;
 };
 
 // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
@@ -55,6 +55,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien
   typedef NoiseModelFactorN<OrientedPlane3> Base;
 
  public:
+  using Base::evaluateError;
   typedef OrientedPlane3DirectionPrior This;
   /// Constructor
   OrientedPlane3DirectionPrior() {
@@ -72,8 +73,7 @@ class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<Orien
   /// equals
   bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
-  Vector evaluateError(const OrientedPlane3& plane,
-      OptionalMatrixType H = OptionalNone) const override;
+  Vector evaluateError(const OrientedPlane3& plane, OptionalMatrixType H) const override;
 };
 
 } // gtsam
diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h
index 66febaee5..1d30ac859 100644
--- a/gtsam/slam/PoseRotationPrior.h
+++ b/gtsam/slam/PoseRotationPrior.h
@@ -25,6 +25,8 @@ public:
   typedef typename POSE::Translation Translation;
   typedef typename POSE::Rotation Rotation;
 
+  using Base::evaluateError;
+
   GTSAM_CONCEPT_POSE_TYPE(Pose)
   GTSAM_CONCEPT_GROUP_TYPE(Pose)
   GTSAM_CONCEPT_LIE_TYPE(Rotation)
@@ -75,7 +77,7 @@ public:
   }
 
   /** h(x)-z */
-  Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override {
     const Rotation& newR = pose.rotation();
     if (H) {
       *H = Matrix::Zero(rDim, xDim);
diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h
index fe1347e3c..6f2982c28 100644
--- a/gtsam/slam/PoseTranslationPrior.h
+++ b/gtsam/slam/PoseTranslationPrior.h
@@ -25,6 +25,8 @@ public:
   typedef POSE Pose;
   typedef typename POSE::Translation Translation;
   typedef typename POSE::Rotation Rotation;
+  
+  using Base::evaluateError;
 
   GTSAM_CONCEPT_POSE_TYPE(Pose)
   GTSAM_CONCEPT_GROUP_TYPE(Pose)
@@ -59,7 +61,7 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** h(x)-z */
-  Vector evaluateError(const Pose& pose, OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Pose& pose, OptionalMatrixType H) const override {
     const Translation& newTrans = pose.translation();
     const Rotation& R = pose.rotation();
     const int tDim = traits<Translation>::GetDimension(newTrans);
diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h
index fa5e1d5fc..4e9632644 100644
--- a/gtsam/slam/ProjectionFactor.h
+++ b/gtsam/slam/ProjectionFactor.h
@@ -60,6 +60,8 @@ namespace gtsam {
     /// shorthand for a smart pointer to a factor
     typedef boost::shared_ptr<This> shared_ptr;
 
+	using Base::evaluateError;
+
     /// Default constructor
     GenericProjectionFactor() :
         measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
@@ -133,7 +135,7 @@ namespace gtsam {
 
     /// Evaluate error h(x)-z and optionally derivatives
     Vector evaluateError(const Pose3& pose, const Point3& point,
-        OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2) const override {
       try {
         if(body_P_sensor_) {
           if(H1) {
diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h
index 9072d8e6e..ed7af6ad2 100644
--- a/gtsam/slam/ReferenceFrameFactor.h
+++ b/gtsam/slam/ReferenceFrameFactor.h
@@ -29,8 +29,8 @@ namespace gtsam {
 template<class T, class P>
 P transform_point(
     const T& trans, const P& global,
-    boost::optional<Matrix&> Dtrans,
-    boost::optional<Matrix&> Dglobal) {
+    OptionalMatrixType Dtrans,
+    OptionalMatrixType Dglobal) {
   return trans.transformFrom(global, Dtrans, Dglobal);
 }
 
diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h
index 47727fa85..33336bf2a 100644
--- a/gtsam/slam/RotateFactor.h
+++ b/gtsam/slam/RotateFactor.h
@@ -28,6 +28,7 @@ class RotateFactor: public NoiseModelFactorN<Rot3> {
   typedef RotateFactor This;
 
 public:
+  using Base::evaluateError;
 
   /// Constructor
   RotateFactor(Key key, const Rot3& P, const Rot3& Z,
@@ -50,8 +51,7 @@ public:
   }
 
   /// vector of errors returns 2D vector
-  Vector evaluateError(const Rot3& R,
-      OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Rot3& R, OptionalMatrixType H) const override {
     // predict p_ as q = R*z_, derivative H will be filled if not none
     Point3 q = R.rotate(z_,H);
     // error is just difference, and note derivative of that wrpt q is I3
@@ -72,6 +72,7 @@ class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
   typedef RotateDirectionsFactor This;
 
 public:
+  using Base::evaluateError;
 
   /// Constructor
   RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
@@ -102,7 +103,7 @@ public:
   }
 
   /// vector of errors returns 2D vector
-  Vector evaluateError(const Rot3& iRc, OptionalMatrixType H = OptionalNone) const override {
+  Vector evaluateError(const Rot3& iRc, OptionalMatrixType H) const override {
     Unit3 i_q = iRc * c_z_;
     Vector error = i_p_.error(i_q, H);
     if (H) {
diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h
index 5e0f7d168..0afe67b13 100644
--- a/gtsam/slam/StereoFactor.h
+++ b/gtsam/slam/StereoFactor.h
@@ -48,6 +48,9 @@ public:
   typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  ///< typedef for shared pointer to this object
   typedef POSE CamPose;                                       ///< typedef for Pose Lie Value type
 
+  using Base::evaluateError;
+	
+
   /**
    * Default constructor
    */
@@ -120,7 +123,7 @@ public:
 
   /** h(x)-z */
   Vector evaluateError(const Pose3& pose, const Point3& point,
-      OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
+      OptionalMatrixType H1, OptionalMatrixType H2) const override {
     try {
       if(body_P_sensor_) {
         if(H1) {
diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h
index 34a67d8a9..98c972b9c 100644
--- a/gtsam/slam/TriangulationFactor.h
+++ b/gtsam/slam/TriangulationFactor.h
@@ -61,6 +61,7 @@ public:
 
   /// shorthand for a smart pointer to a factor
   using shared_ptr = boost::shared_ptr<This>;
+  using NoiseModelFactor1<Point3>::evaluateError;
 
   /// Default constructor
   TriangulationFactor() :
@@ -119,8 +120,7 @@ public:
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  Vector evaluateError(const Point3& point, OptionalMatrixType H2 =
-      OptionalNone) const override {
+  Vector evaluateError(const Point3& point, OptionalMatrixType H2) const override {
     try {
       return traits<Measurement>::Local(measured_, camera_.project2(point, OptionalNone, H2));
     } catch (CheiralityException& e) {
diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp
index 6897943ec..68a7f63cb 100644
--- a/gtsam/slam/tests/testBetweenFactor.cpp
+++ b/gtsam/slam/tests/testBetweenFactor.cpp
@@ -36,16 +36,13 @@ TEST(BetweenFactor, Rot3) {
   EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail
 
   Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
-      std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
-          &BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1,
-          std::placeholders::_2, boost::none, boost::none)),
+        [&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);},
       R1, R2, 1e-5);
   EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
 
   Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
-      std::function<Vector(const Rot3&, const Rot3&)>(std::bind(
-          &BetweenFactor<Rot3>::evaluateError, factor, std::placeholders::_1, std::placeholders::_2, boost::none,
-          boost::none)), R1, R2, 1e-5);
+        [&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);},
+        R1, R2, 1e-5);
   EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
 }
 
diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
index 2faac24d1..c3c4432c0 100644
--- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
+++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp
@@ -53,12 +53,11 @@ TEST( EssentialMatrixConstraint, test ) {
 
   // Calculate numerical derivatives
   Matrix expectedH1 = numericalDerivative11<Vector5, Pose3>(
-      std::bind(&EssentialMatrixConstraint::evaluateError, &factor,
-                std::placeholders::_1, pose2, boost::none, boost::none),
-      pose1);
+		[&factor, &pose2](const Pose3& p1) {return factor.evaluateError(p1, pose2);},
+		pose1);
+
   Matrix expectedH2 = numericalDerivative11<Vector5, Pose3>(
-      std::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1,
-                std::placeholders::_1, boost::none, boost::none),
+		[&factor, &pose1](const Pose3& p2) {return factor.evaluateError(pose1, p2);},
       pose2);
 
   // Use the factor to calculate the derivative
diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
index bc3f7ce94..e393ee16b 100644
--- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp
+++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp
@@ -140,9 +140,9 @@ TEST( OrientedPlane3Factor, Derivatives ) {
   OrientedPlane3Factor factor(p.planeCoefficients(), noise, poseKey, planeKey);
 
   // Calculate numerical derivatives
-  std::function<Vector(const Pose3 &, const OrientedPlane3 &)> f = std::bind(
-      &OrientedPlane3Factor::evaluateError, factor, std::placeholders::_1,
-      std::placeholders::_2, boost::none, boost::none);
+  std::function<Vector(const Pose3&, const OrientedPlane3&)> f = [&factor](const Pose3& p, const OrientedPlane3& o) {
+    return factor.evaluateError(p, o);
+  };
   Matrix numericalH1 = numericalDerivative21<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
   Matrix numericalH2 = numericalDerivative22<Vector, Pose3, OrientedPlane3>(f, poseLin, pLin);
 
@@ -180,16 +180,13 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
 
   // Calculate numerical derivatives
   Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
-      std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
-          boost::none), T1);
+		  [&factor](const OrientedPlane3& o) {return factor.evaluateError(o);}, T1);
 
   Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
-      std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
-          boost::none), T2);
+      [&factor](const OrientedPlane3& o) {return factor.evaluateError(o);}, T2);
 
   Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
-      std::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, std::placeholders::_1,
-          boost::none), T3);
+      [&factor](const OrientedPlane3& o) { return factor.evaluateError(o); }, T3);
 
   // Use the factor to calculate the derivative
   Matrix actualH1, actualH2, actualH3;
diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp
index 01bfc866b..68e0de0f0 100644
--- a/gtsam/slam/tests/testRotateFactor.cpp
+++ b/gtsam/slam/tests/testRotateFactor.cpp
@@ -69,14 +69,14 @@ TEST (RotateFactor, test) {
   Matrix actual, expected;
   // Use numerical derivatives to calculate the expected Jacobian
   {
-    expected = numericalDerivative11<Vector3,Rot3>(
-        std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), iRc);
+    expected = numericalDerivative11<Vector3, Rot3>(
+			[&f](const Rot3& r) { return f.evaluateError(r); }, iRc);
     f.evaluateError(iRc, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
   }
   {
-    expected = numericalDerivative11<Vector3,Rot3>(
-        std::bind(&RotateFactor::evaluateError, &f, std::placeholders::_1, boost::none), R);
+    expected = numericalDerivative11<Vector3, Rot3>(
+			[&f](const Rot3& r) { return f.evaluateError(r); }, R);
     f.evaluateError(R, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
   }
@@ -141,15 +141,13 @@ TEST (RotateDirectionsFactor, test) {
   // Use numerical derivatives to calculate the expected Jacobian
   {
     expected = numericalDerivative11<Vector,Rot3>(
-        std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
-            boost::none), iRc);
+			[&f](const Rot3& r) {return f.evaluateError(r);}, iRc);
     f.evaluateError(iRc, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
   }
   {
     expected = numericalDerivative11<Vector,Rot3>(
-        std::bind(&RotateDirectionsFactor::evaluateError, &f, std::placeholders::_1,
-            boost::none), R);
+			[&f](const Rot3& r) {return f.evaluateError(r);}, R);
     f.evaluateError(R, actual);
     EXPECT(assert_equal(expected, actual, 1e-9));
   }
diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp
index 08d8e66c0..42df4d8fe 100644
--- a/gtsam/slam/tests/testTriangulationFactor.cpp
+++ b/gtsam/slam/tests/testTriangulationFactor.cpp
@@ -59,7 +59,7 @@ TEST( triangulation, TriangulationFactor ) {
   factor.evaluateError(landmark, HActual);
 
   Matrix HExpected = numericalDerivative11<Vector,Point3>(
-      std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
+		  [&factor](const Point3& l) { return factor.evaluateError(l);}, landmark);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(HExpected, HActual, 1e-3));
@@ -82,8 +82,8 @@ TEST( triangulation, TriangulationFactorStereo ) {
   Matrix HActual;
   factor.evaluateError(landmark, HActual);
 
-  Matrix HExpected = numericalDerivative11<Vector,Point3>(
-      std::bind(&Factor::evaluateError, &factor, std::placeholders::_1, boost::none), landmark);
+  Matrix HExpected = numericalDerivative11<Vector, Point3>(
+		  [&factor](const Point3& l) { return factor.evaluateError(l);}, landmark);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(HExpected, HActual, 1e-3));
diff --git a/tests/simulated2D.h b/tests/simulated2D.h
index bce27ac3e..b4f78ee3b 100644
--- a/tests/simulated2D.h
+++ b/tests/simulated2D.h
@@ -21,6 +21,7 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/geometry/Point2.h>
+#include "gtsam/base/OptionalJacobian.h"
 
 // \namespace
 
@@ -89,7 +90,7 @@ namespace simulated2D {
   }
 
   /// Prior on a single pose, optionally returns derivative
-  inline Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
+  inline Point2 prior(const Point2& x, OptionalJacobian<2,2> H = OptionalNone) {
     if (H) *H = I_2x2;
     return x;
   }
@@ -100,8 +101,9 @@ namespace simulated2D {
   }
 
   /// odometry between two poses, optionally returns derivative
-  inline Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
-    boost::none, boost::optional<Matrix&> H2 = boost::none) {
+  inline Point2 odo(const Point2& x1, const Point2& x2, 
+		  OptionalJacobian<2,2> H1 = OptionalNone, 
+		  OptionalJacobian<2,2> H2 = OptionalNone) {
       if (H1) *H1 = -I_2x2;
       if (H2) *H2 = I_2x2;
       return x2 - x1;
@@ -113,8 +115,8 @@ namespace simulated2D {
   }
 
   /// measurement between landmark and pose, optionally returns derivative
-  inline Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
-    boost::none, boost::optional<Matrix&> H2 = boost::none) {
+  inline Point2 mea(const Point2& x, const Point2& l, OptionalJacobian<2,2> H1 =
+    OptionalNone, OptionalMatrixType H2 = OptionalNone) {
       if (H1) *H1 = -I_2x2;
       if (H2) *H2 = I_2x2;
       return l - x;
@@ -130,6 +132,8 @@ namespace simulated2D {
     typedef GenericPrior<VALUE> This;
     typedef boost::shared_ptr<GenericPrior<VALUE> > shared_ptr;
     typedef VALUE Pose; ///< shortcut to Pose type
+	
+    using Base::evaluateError;
 
     Pose measured_; ///< prior mean
 
@@ -139,7 +143,7 @@ namespace simulated2D {
     }
 
     /// Return error and optional derivative
-    Vector evaluateError(const Pose& x, OptionalMatrixType H = OptionalNone) const override {
+    Vector evaluateError(const Pose& x, OptionalMatrixType H) const override {
       return (prior(x, H) - measured_);
     }
 
@@ -175,6 +179,8 @@ namespace simulated2D {
     typedef boost::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
     typedef VALUE Pose; ///< shortcut to Pose type
 
+    using Base::evaluateError;
+
     Pose measured_; ///< odometry measurement
 
     /// Create odometry
@@ -184,8 +190,7 @@ namespace simulated2D {
 
     /// Evaluate error and optionally return derivatives
     Vector evaluateError(const Pose& x1, const Pose& x2,
-        OptionalMatrixType H1 = OptionalNone,
-        OptionalMatrixType H2 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2) const override {
       return (odo(x1, x2, H1, H2) - measured_);
     }
 
@@ -222,6 +227,8 @@ namespace simulated2D {
     typedef POSE Pose; ///< shortcut to Pose type
     typedef LANDMARK Landmark; ///< shortcut to Landmark type
 
+    using Base::evaluateError;
+
     Landmark measured_; ///< Measurement
 
     /// Create measurement factor
@@ -231,8 +238,7 @@ namespace simulated2D {
 
     /// Evaluate error and optionally return derivatives
     Vector evaluateError(const Pose& x1, const Landmark& x2,
-        OptionalMatrixType H1 = OptionalNone,
-        OptionalMatrixType H2 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2) const override {
       return (mea(x1, x2, H1, H2) - measured_);
     }
 
diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h
index 11c2fd9a1..c9ac38f82 100644
--- a/tests/simulated2DOriented.h
+++ b/tests/simulated2DOriented.h
@@ -22,6 +22,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include "gtsam/base/OptionalJacobian.h"
 
 // \namespace
 namespace simulated2DOriented {
@@ -62,7 +63,7 @@ namespace simulated2DOriented {
   }
 
   /// Prior on a single pose, optional derivative version
-  Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none) {
+  Pose2 prior(const Pose2& x, OptionalJacobian<3,3> H = OptionalNone) {
     if (H) *H = I_3x3;
     return x;
   }
@@ -73,8 +74,8 @@ namespace simulated2DOriented {
   }
 
   /// odometry between two poses, optional derivative version
-  Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional<Matrix&> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) {
+  Pose2 odo(const Pose2& x1, const Pose2& x2, OptionalJacobian<3,3> H1 =
+      OptionalNone, OptionalJacobian<3,3> H2 = OptionalNone) {
     return x1.between(x2, H1, H2);
   }
 
@@ -105,6 +106,7 @@ namespace simulated2DOriented {
     Pose2 measured_;   ///< Between measurement for odometry factor
 
     typedef GenericOdometry<VALUE> This;
+	using NoiseModelFactor2<VALUE, VALUE>::evaluateError;
 
     /**
      * Creates an odometry factor between two poses
@@ -118,8 +120,7 @@ namespace simulated2DOriented {
 
     /// Evaluate error and optionally derivative
     Vector evaluateError(const VALUE& x1, const VALUE& x2,
-        OptionalMatrixType H1 = OptionalNone,
-        OptionalMatrixType H2 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2) const override {
       return measured_.localCoordinates(odo(x1, x2, H1, H2));
     }
 
diff --git a/tests/simulated3D.h b/tests/simulated3D.h
index f65e8cfc3..5cec78e4e 100644
--- a/tests/simulated3D.h
+++ b/tests/simulated3D.h
@@ -23,6 +23,7 @@
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include "gtsam/base/OptionalJacobian.h"
 
 // \namespace
 
@@ -38,7 +39,7 @@ namespace simulated3D {
 /**
  * Prior on a single pose
  */
-Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none) {
+Point3 prior(const Point3& x, OptionalJacobian<3,3> H = OptionalNone) {
   if (H) *H = I_3x3;
   return x;
 }
@@ -47,8 +48,8 @@ Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none) {
  * odometry between two poses
  */
 Point3 odo(const Point3& x1, const Point3& x2,
-    boost::optional<Matrix&> H1 = boost::none,
-    boost::optional<Matrix&> H2 = boost::none) {
+    OptionalJacobian<3,3> H1 = OptionalNone,
+    OptionalJacobian<3,3> H2 = OptionalNone) {
   if (H1) *H1 = -1 * I_3x3;
   if (H2) *H2 = I_3x3;
   return x2 - x1;
@@ -58,8 +59,8 @@ Point3 odo(const Point3& x1, const Point3& x2,
  *  measurement between landmark and pose
  */
 Point3 mea(const Point3& x, const Point3& l,
-    boost::optional<Matrix&> H1 = boost::none,
-    boost::optional<Matrix&> H2 = boost::none) {
+    OptionalJacobian<3,3> H1 = OptionalNone,
+    OptionalJacobian<3,3> H2 = OptionalNone) {
   if (H1) *H1 = -1 * I_3x3;
   if (H2) *H2 = I_3x3;
   return l - x;
@@ -68,7 +69,8 @@ Point3 mea(const Point3& x, const Point3& l,
 /**
  * A prior factor on a single linear robot pose
  */
-struct PointPrior3D: public NoiseModelFactorN<Point3> {
+struct PointPrior3D: public NoiseModelFactor1<Point3> {
+  using NoiseModelFactor1<Point3>::evaluateError;
 
   Point3 measured_; ///< The prior pose value for the variable attached to this factor
 
@@ -89,8 +91,7 @@ struct PointPrior3D: public NoiseModelFactorN<Point3> {
    * @param H is an optional Jacobian matrix (Dimension: 3x3)
    * @return Vector error between prior value and x (Dimension: 3)
    */
-  Vector evaluateError(const Point3& x, OptionalMatrixType H =
-      OptionalNone) const override {
+  Vector evaluateError(const Point3& x, OptionalMatrixType H) const override {
     return prior(x, H) - measured_;
   }
 };
@@ -98,7 +99,12 @@ struct PointPrior3D: public NoiseModelFactorN<Point3> {
 /**
  * Models a linear 3D measurement between 3D points
  */
+<<<<<<< HEAD
 struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
+=======
+struct Simulated3DMeasurement: public NoiseModelFactor2<Point3, Point3> {
+  using NoiseModelFactor2<Point3, Point3>::evaluateError;
+>>>>>>> 7b3c40e92 (everything compiles but tests fail in no boost mode)
 
   Point3 measured_; ///< Linear displacement between a pose and landmark
 
@@ -121,7 +127,7 @@ struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
    * @return vector error between measurement and prediction (Dimension: 3)
    */
   Vector evaluateError(const Point3& x1, const Point3& x2,
-      OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const override {
+      OptionalMatrixType H1, OptionalMatrixType H2) const override {
     return mea(x1, x2, H1, H2) - measured_;
   }
 };
diff --git a/tests/smallExample.h b/tests/smallExample.h
index 3eab85b49..2030c973b 100644
--- a/tests/smallExample.h
+++ b/tests/smallExample.h
@@ -329,13 +329,14 @@ inline Matrix H(const Point2& v) {
 
 struct UnaryFactor: public gtsam::NoiseModelFactorN<Point2> {
 
+  using gtsam::NoiseModelFactor1<Point2>::evaluateError;
   Point2 z_;
 
   UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) :
     gtsam::NoiseModelFactorN<Point2>(model, key), z_(z) {
   }
 
-  Vector evaluateError(const Point2& x, OptionalMatrixType A = OptionalNone) const override {
+  Vector evaluateError(const Point2& x, OptionalMatrixType A) const override {
     if (A) *A = H(x);
     return (h(x) - z_);
   }
diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp
index f6fcce1e2..dc6f08635 100644
--- a/tests/testExtendedKalmanFilter.cpp
+++ b/tests/testExtendedKalmanFilter.cpp
@@ -101,6 +101,8 @@ protected:
   Matrix Q_invsqrt_;
 
 public:
+  using Base::evaluateError;
+
   NonlinearMotionModel(){}
 
   NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) :
@@ -213,8 +215,7 @@ public:
 
   /** vector of errors */
   Vector evaluateError(const Point2& p1, const Point2& p2,
-      OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 =
-          OptionalNone) const override {
+      OptionalMatrixType H1, OptionalMatrixType H2) const override {
 
     // error = p2 - f(p1)
     // H1 = d error / d p1 = -d f/ d p1 = -F
@@ -245,6 +246,7 @@ protected:
   Matrix R_invsqrt_; /** The inv sqrt of the measurement error covariance */
 
 public:
+  using Base::evaluateError;
   NonlinearMeasurementModel(){}
 
   NonlinearMeasurementModel(const Symbol& TestKey, Vector z) :
@@ -339,7 +341,7 @@ public:
   }
 
   /** vector of errors */
-  Vector evaluateError(const Point2& p, OptionalMatrixType H1 = OptionalNone) const override {
+  Vector evaluateError(const Point2& p, OptionalMatrixType H1) const override {
     // error = z - h(p)
     // H = d error / d p = -d h/ d p = -H
     Vector z_hat = h(p);
diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp
index 43912d703..ee0660866 100644
--- a/tests/testNonlinearFactor.cpp
+++ b/tests/testNonlinearFactor.cpp
@@ -337,10 +337,11 @@ class TestFactor1 : public NoiseModelFactor1<double> {
 
  public:
   typedef NoiseModelFactor1<double> Base;
+  using Base::evaluateError;
   TestFactor1() : Base(noiseModel::Diagonal::Sigmas(Vector1(2.0)), L(1)) {}
   using Base::NoiseModelFactor1;  // inherit constructors
 
-  Vector evaluateError(const double& x1, OptionalMatrixType H1 = OptionalNone) const override {
+  Vector evaluateError(const double& x1, OptionalMatrixType H1) const override {
     if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
     return (Vector(1) << x1).finished();
   }
@@ -388,15 +389,14 @@ class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
 
  public:
   typedef NoiseModelFactor4<double, double, double, double> Base;
+  using Base::evaluateError;
   TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
   using Base::NoiseModelFactor4;  // inherit constructors
 
   Vector
     evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
-        OptionalMatrixType H1 = OptionalNone,
-        OptionalMatrixType H2 = OptionalNone,
-        OptionalMatrixType H3 = OptionalNone,
-        OptionalMatrixType H4 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2,
+        OptionalMatrixType H3, OptionalMatrixType H4) const override {
     if(H1) {
       *H1 = (Matrix(1, 1) << 1.0).finished();
       *H2 = (Matrix(1, 1) << 2.0).finished();
@@ -479,15 +479,13 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
 class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
 public:
   typedef NoiseModelFactor5<double, double, double, double, double> Base;
+  using Base::evaluateError;
   TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5)) {}
 
   Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
-        OptionalMatrixType H1 = OptionalNone,
-        OptionalMatrixType H2 = OptionalNone,
-        OptionalMatrixType H3 = OptionalNone,
-        OptionalMatrixType H4 = OptionalNone,
-        OptionalMatrixType H5 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, 
+		OptionalMatrixType H4, OptionalMatrixType H5) const override {
     if(H1) {
       *H1 = (Matrix(1, 1) << 1.0).finished();
       *H2 = (Matrix(1, 1) << 2.0).finished();
@@ -529,16 +527,13 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
 class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
 public:
   typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
+  using Base::evaluateError;
   TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4), X(5), X(6)) {}
 
   Vector
     evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
-        OptionalMatrixType H1 = OptionalNone,
-        OptionalMatrixType H2 = OptionalNone,
-        OptionalMatrixType H3 = OptionalNone,
-        OptionalMatrixType H4 = OptionalNone,
-        OptionalMatrixType H5 = OptionalNone,
-        OptionalMatrixType H6 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, 
+		OptionalMatrixType H5, OptionalMatrixType H6) const override {
     if(H1) {
       *H1 = (Matrix(1, 1) << 1.0).finished();
       *H2 = (Matrix(1, 1) << 2.0).finished();
@@ -587,16 +582,15 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
 class TestFactorN : public NoiseModelFactorN<double, double, double, double> {
 public:
   typedef NoiseModelFactorN<double, double, double, double> Base;
+  using Base::evaluateError;
   using Type1 = ValueType<1>;  // Test that we can use the ValueType<> template
 
   TestFactorN() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0).finished()), X(1), X(2), X(3), X(4)) {}
 
   Vector
     evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
-        OptionalMatrixType H1 = OptionalNone,
-        OptionalMatrixType H2 = OptionalNone,
-        OptionalMatrixType H3 = OptionalNone,
-        OptionalMatrixType H4 = OptionalNone) const override {
+        OptionalMatrixType H1, OptionalMatrixType H2,
+        OptionalMatrixType H3, OptionalMatrixType H4) const override {
     if (H1) *H1 = (Matrix(1, 1) << 1.0).finished();
     if (H2) *H2 = (Matrix(1, 1) << 2.0).finished();
     if (H3) *H3 = (Matrix(1, 1) << 3.0).finished();