addressed MR comments on nonlinearfactor

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 = OptionalMatrixVecNone) const override;
+  Vector unwhitenedError(const Values &x, OptionalMatrixVecType H = nullptr) const override;
 
   /** print */
   void print(const std::string &s,

gtsam/nonlinear/ExpressionFactor.h

@@ -99,7 +99,7 @@ protected:
    * both the function evaluation and its derivative(s) in H.
    */
   Vector unwhitenedError(const Values& x,
-    OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
+    OptionalMatrixVecType H = nullptr) 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

@@ -29,25 +29,32 @@
 #include <gtsam/base/utilities.h>  // boost::index_sequence
 
 #include <boost/serialization/base_object.hpp>
+#include <cstddef>
 
 namespace gtsam {
 
 /* ************************************************************************* */
 
-// These typedefs and aliases will help with making the evaluateError interface
+/* These typedefs and aliases will help with making the evaluateError interface
-// independent of boost
+* independent of boost
-using OptionalNoneType = std::nullptr_t;
+* TODO(kartikarcot): Change this to OptionalMatrixNone
-// TODO: Change this to OptionalMatrixNone
+* This typedef is used to indicate that the Jacobian is not required
+* and the default value used for optional matrix pointer arguments in evaluateError.
+* Had to use the static_cast of a nullptr, because the compiler is not able to
+* deduce the type of the nullptr when expanding the evaluateError templates.
+*/
 #define OptionalNone static_cast<Matrix*>(nullptr)
-template <typename T = void>
+
-using OptionalMatrixTypeT = Matrix*;
+/* This typedef will be used everywhere boost::optional<Matrix&> reference was used
-template <typename T = void>
+ * previously. This is used to indicate that the Jacobian is optional. In the future
-using MatrixTypeT = Matrix;
+ * we will change this to OptionalJacobian
+ */
 using OptionalMatrixType = Matrix*;
-// These typedefs and aliases will help with making the unwhitenedError interface
+
-// independent of boost
+/* The OptionalMatrixVecType is a pointer to a vector of matrices. It will
+ * be used in situations where a vector of matrices is optional, like in 
+ * unwhitenedError. */
 using OptionalMatrixVecType = std::vector<Matrix>*;
-#define OptionalMatrixVecNone static_cast<std::vector<Matrix>*>(nullptr)
 
 /**
  * Nonlinear factor base class
@@ -244,7 +251,7 @@ 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 = OptionalMatrixVecNone) const = 0;
+  virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
 
   // support taking in the actual vector instead of the pointer as well
   // to get access to this version of the function from derived classes
@@ -452,13 +459,22 @@ protected:
   /* Like std::void_t, except produces `OptionalMatrixType` instead of
    * `void`. Used to expand fixed-type parameter-packs with same length as
    * ValueTypes. */
+  template <typename T = void>
+  using OptionalMatrixTypeT = Matrix*;
 
   /* Like std::void_t, except produces `Key` instead of `void`. Used to expand
    * fixed-type parameter-packs with same length as ValueTypes. */
   template <typename T>
   using KeyType = Key;
 
- public:
+  /* Like std::void_t, except produces `Matrix` instead of
+   * `void`. Used to expand fixed-type parameter-packs with same length as
+   * ValueTypes. This helps in creating an evaluateError overload that accepts
+   * Matrices instead of pointers to matrices */
+  template <typename T = void>
+  using MatrixTypeT = Matrix;
+
+  public:
   /**
    * The type of the I'th template param can be obtained as ValueType<I>.
    * I is 1-indexed for backwards compatibility/consistency!  So for example,
@@ -563,7 +579,7 @@ protected:
    */
   Vector unwhitenedError(
       const Values& x,
-      OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
+      OptionalMatrixVecType H = nullptr) const override {
     return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
                            H);
   }
@@ -603,6 +619,7 @@ protected:
   // one will need to use the "using" keyword and specify that like this:
   // public:
   //   using NoiseModelFactorN<list the value types here>::evaluateError;
+
   Vector evaluateError(const ValueTypes&... x, MatrixTypeT<ValueTypes>&... H) const {
     return evaluateError(x..., (&H)...);
   }
@@ -631,7 +648,7 @@ protected:
     constexpr bool are_all_mat = (... && (std::is_same<Matrix, std::decay_t<OptionalJacArgs>>::value));
     // The pointers can either be of std::nonetype_t or of Matrix* type
     constexpr bool are_all_ptrs = (... && (std::is_same<OptionalMatrixType, std::decay_t<OptionalJacArgs>>::value ||
-                                           std::is_same<OptionalNoneType, std::decay_t<OptionalJacArgs>>::value));
+                                           std::is_same<std::nullptr_t, 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*");
@@ -659,7 +676,7 @@ protected:
   inline Vector unwhitenedError(
       boost::mp11::index_sequence<Indices...>,  //
       const Values& x,
-      OptionalMatrixVecType H = OptionalMatrixVecNone) const {
+      OptionalMatrixVecType H = nullptr) const {
     if (this->active(x)) {
       if (H) {
         return evaluateError(x.at<ValueTypes>(keys_[Indices])...,

gtsam/sam/RangeFactor.h

@@ -63,7 +63,9 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
                                OptionalMatrixType H2 = OptionalNone) const {
     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 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;

gtsam_unstable/slam/BetweenFactorEM.h

@@ -145,7 +145,7 @@ public:
 
   /* ************************************************************************* */
   Vector whitenedError(const Values& x,
-      OptionalMatrixVecType H = OptionalMatrixVecNone) const {
+      OptionalMatrixVecType H = nullptr) const {
 
     bool debug = true;
 

gtsam_unstable/slam/MultiProjectionFactor.h

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

gtsam_unstable/slam/SmartRangeFactor.h

@@ -146,7 +146,7 @@ class SmartRangeFactor: public NoiseModelFactor {
   /**
    * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
    */
-  Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const override {
+  Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
     size_t n = size();
     if (n < 3) {
       if (H) {

gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h

@@ -156,7 +156,7 @@ namespace gtsam {
 
 
     /* ************************************************************************* */
-    gtsam::Vector whitenedError(const gtsam::Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const {
+    gtsam::Vector whitenedError(const gtsam::Values& x, OptionalMatrixVecType H = nullptr) const {
 
       T orgA_T_currA = valA_.at<T>(keyA_);
       T orgB_T_currB = valB_.at<T>(keyB_);

gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h

@@ -179,7 +179,7 @@ namespace gtsam {
 
 
     /* ************************************************************************* */
-    Vector whitenedError(const Values& x, OptionalMatrixVecType H = OptionalMatrixVecNone) const {
+    Vector whitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const {
 
       bool debug = true;