Some low hanging fruit changing `func(..., boost::optional<const Class &>)` to overloaded

`func(...)` and
`func(..., const Class &)`
not all done
Also, although it currently passes all tests, I think some more tests may be needed...

gtsam/base/TestableAssertions.h

@@ -71,12 +71,8 @@ bool assert_equal(const V& expected, const boost::optional<V>& actual, double to
 }
 
 template<class V>
-bool assert_equal(const V& expected, const boost::optional<const V&>& actual, double tol = 1e-9) {
+bool assert_equal(const V& expected, double tol = 1e-9) {
-  if (!actual) {
+  return false;
-    std::cout << "actual is boost::none" << std::endl;
-    return false;
-  }
-  return assert_equal(expected, *actual, tol);
 }
 
 /**

gtsam/inference/EliminateableFactorGraph.h

@@ -89,10 +89,10 @@ namespace gtsam {
     typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
 
     /// Typedef for an optional ordering as an argument to elimination functions
-    typedef boost::optional<const Ordering&> OptionalOrdering;
+    typedef const boost::optional<Ordering>& OptionalOrdering;
 
     /// Typedef for an optional variable index as an argument to elimination functions
-    typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
+    typedef const boost::optional<VariableIndex>& OptionalVariableIndex;
 
     /// Typedef for an optional ordering type
     typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;

gtsam/linear/GaussianBayesNet.cpp

@@ -156,16 +156,17 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  pair<Matrix, Vector> GaussianBayesNet::matrix(boost::optional<const Ordering&> ordering) const {
+  pair<Matrix, Vector> GaussianBayesNet::matrix(const Ordering& ordering) const {
-    if (ordering) {
+    // Convert to a GaussianFactorGraph and use its machinery
-      // Convert to a GaussianFactorGraph and use its machinery
+    GaussianFactorGraph factorGraph(*this);
-      GaussianFactorGraph factorGraph(*this);
+    return factorGraph.jacobian(ordering);
-      return factorGraph.jacobian(ordering);
+  }
-    } else {
+
-      // recursively call with default ordering
+  /* ************************************************************************* */
-      const auto defaultOrdering = this->ordering();
+  pair<Matrix, Vector> GaussianBayesNet::matrix() const {
-      return matrix(defaultOrdering);
+    // recursively call with default ordering
-    }
+    const auto defaultOrdering = this->ordering();
+    return matrix(defaultOrdering);
   }
 
   ///* ************************************************************************* */

gtsam/linear/GaussianBayesNet.h

@@ -92,7 +92,14 @@ namespace gtsam {
      * Will return upper-triangular matrix only when using 'ordering' above.
      * In case Bayes net is incomplete zero columns are added to the end.
      */
-    std::pair<Matrix, Vector> matrix(boost::optional<const Ordering&> ordering = boost::none) const;
+    std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
+
+    /**
+     * Return (dense) upper-triangular matrix representation
+     * Will return upper-triangular matrix only when using 'ordering' above.
+     * In case Bayes net is incomplete zero columns are added to the end.
+     */
+    std::pair<Matrix, Vector> matrix() const;
 
     /**
      * Optimize along the gradient direction, with a closed-form computation to perform the line

gtsam/linear/GaussianFactorGraph.cpp

@@ -190,33 +190,62 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedJacobian(
-      boost::optional<const Ordering&> optionalOrdering) const {
+      const Ordering& ordering) const {
     // combine all factors
-    JacobianFactor combined(*this, optionalOrdering);
+    JacobianFactor combined(*this, ordering);
+    return combined.augmentedJacobian();
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::augmentedJacobian() const {
+    // combine all factors
+    JacobianFactor combined(*this);
     return combined.augmentedJacobian();
   }
 
   /* ************************************************************************* */
   pair<Matrix, Vector> GaussianFactorGraph::jacobian(
-      boost::optional<const Ordering&> optionalOrdering) const {
+      const Ordering& ordering) const {
-    Matrix augmented = augmentedJacobian(optionalOrdering);
+    Matrix augmented = augmentedJacobian(ordering);
+    return make_pair(augmented.leftCols(augmented.cols() - 1),
+        augmented.col(augmented.cols() - 1));
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianFactorGraph::jacobian() const {
+    Matrix augmented = augmentedJacobian();
     return make_pair(augmented.leftCols(augmented.cols() - 1),
         augmented.col(augmented.cols() - 1));
   }
 
   /* ************************************************************************* */
   Matrix GaussianFactorGraph::augmentedHessian(
-      boost::optional<const Ordering&> optionalOrdering) const {
+      const Ordering& ordering) const {
     // combine all factors and get upper-triangular part of Hessian
-    Scatter scatter(*this, optionalOrdering);
+    Scatter scatter(*this, ordering);
+    HessianFactor combined(*this, scatter);
+    return combined.info().selfadjointView();;
+  }
+
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::augmentedHessian() const {
+    // combine all factors and get upper-triangular part of Hessian
+    Scatter scatter(*this);
     HessianFactor combined(*this, scatter);
     return combined.info().selfadjointView();;
   }
 
   /* ************************************************************************* */
   pair<Matrix, Vector> GaussianFactorGraph::hessian(
-      boost::optional<const Ordering&> optionalOrdering) const {
+      const Ordering& ordering) const {
-    Matrix augmented = augmentedHessian(optionalOrdering);
+    Matrix augmented = augmentedHessian(ordering);
+    size_t n = augmented.rows() - 1;
+    return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianFactorGraph::hessian() const {
+    Matrix augmented = augmentedHessian();
     size_t n = augmented.rows() - 1;
     return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
   }

gtsam/linear/GaussianFactorGraph.h

@@ -201,7 +201,16 @@ namespace gtsam {
      * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
      * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
      */
-    Matrix augmentedJacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    Matrix augmentedJacobian(const Ordering& ordering) const;
+    
+    /**
+     * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
+     * Jacobian matrix, augmented with b with the noise models baked
+     * into A and b.  The negative log-likelihood is
+     * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
+     * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
+     */
+    Matrix augmentedJacobian() const;
 
     /**
      * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
@@ -210,7 +219,16 @@ namespace gtsam {
      * GaussianFactorGraph::augmentedJacobian and
      * GaussianFactorGraph::sparseJacobian.
      */
-    std::pair<Matrix,Vector> jacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
+
+    /**
+     * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
+     * with the noise models baked into A and b. The negative log-likelihood
+     * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
+     * GaussianFactorGraph::augmentedJacobian and
+     * GaussianFactorGraph::sparseJacobian.
+     */
+    std::pair<Matrix,Vector> jacobian() const;
 
     /**
      * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
@@ -223,7 +241,20 @@ namespace gtsam {
      and the negative log-likelihood is
      \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
      */
-    Matrix augmentedHessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    Matrix augmentedHessian(const Ordering& ordering) const;
+
+    /**
+     * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
+     * matrix, augmented with the information vector \f$ \eta \f$.  The
+     * augmented Hessian is
+     \f[ \left[ \begin{array}{ccc}
+     \Lambda & \eta \\
+     \eta^T & c
+     \end{array} \right] \f]
+     and the negative log-likelihood is
+     \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
+     */
+    Matrix augmentedHessian() const;
 
     /**
      * Return the dense Hessian \f$ \Lambda \f$ and information vector
@@ -231,7 +262,15 @@ namespace gtsam {
      * is \frac{1}{2} x^T \Lambda x + \eta^T x + c.  See also
      * GaussianFactorGraph::augmentedHessian.
      */
-    std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
+    std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
+
+    /**
+     * Return the dense Hessian \f$ \Lambda \f$ and information vector
+     * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
+     * is \frac{1}{2} x^T \Lambda x + \eta^T x + c.  See also
+     * GaussianFactorGraph::augmentedHessian.
+     */
+    std::pair<Matrix,Vector> hessian() const;
 
     /** Return only the diagonal of the Hessian A'*A, as a VectorValues */
     virtual VectorValues hessianDiagonal() const;

gtsam/linear/HessianFactor.cpp

@@ -250,10 +250,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) :
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
-    boost::optional<const Scatter&> scatter) {
+    const Scatter& scatter) {
   gttic(HessianFactor_MergeConstructor);
 
-  Allocate(scatter ? *scatter : Scatter(factors));
+  Allocate(scatter);
 
   // Form A' * A
   gttic(update);

gtsam/linear/HessianFactor.h

@@ -176,7 +176,11 @@ namespace gtsam {
 
     /** Combine a set of factors into a single dense HessianFactor */
     explicit HessianFactor(const GaussianFactorGraph& factors,
-      boost::optional<const Scatter&> scatter = boost::none);
+      const Scatter& scatter);
+
+    /** Combine a set of factors into a single dense HessianFactor */
+    explicit HessianFactor(const GaussianFactorGraph& factors)
+        : HessianFactor(factors, Scatter(factors)) {}
 
     /** Destructor */
     virtual ~HessianFactor() {}

gtsam/linear/IterativeSolver.cpp

@@ -84,16 +84,25 @@ string IterativeOptimizationParameters::verbosityTranslator(
 
 /*****************************************************************************/
 VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
-    boost::optional<const KeyInfo&> keyInfo,
+    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
-    boost::optional<const std::map<Key, Vector>&> lambda) {
+  return optimize(gfg, keyInfo, lambda, keyInfo.x0());
-  return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
-      lambda ? *lambda : std::map<Key, Vector>());
 }
 
 /*****************************************************************************/
 VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
-    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
+    const KeyInfo& keyInfo) {
-  return optimize(gfg, keyInfo, lambda, keyInfo.x0());
+  return optimize(gfg, keyInfo, std::map<Key, Vector>());
+}
+
+/*****************************************************************************/
+VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
+    const std::map<Key, Vector>& lambda) {
+  return optimize(gfg, KeyInfo(gfg), lambda);
+}
+
+/*****************************************************************************/
+VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) {
+  return optimize(gfg, KeyInfo(gfg), std::map<Key, Vector>());
 }
 
 /****************************************************************************/

gtsam/linear/IterativeSolver.h

@@ -91,15 +91,21 @@ public:
   virtual ~IterativeSolver() {
   }
 
-  /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
-  GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
-      boost::optional<const KeyInfo&> = boost::none,
-      boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
-
   /* interface to the nonlinear optimizer, without initial estimate */
   GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
       const std::map<Key, Vector> &lambda);
 
+  /* interface to the nonlinear optimizer, without damping and initial estimate */
+  GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
+      const KeyInfo& keyInfo);
+
+  /* interface to the nonlinear optimizer, without metadata and initial estimate */
+  GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
+      const std::map<Key, Vector>& lambda);
+
+  /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
+  GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg);
+
   /* interface to the nonlinear optimizer that the subclasses have to implement */
   virtual VectorValues optimize(const GaussianFactorGraph &gfg,
       const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,

gtsam/linear/JacobianFactor.cpp

@@ -220,60 +220,13 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
 }
 
 /* ************************************************************************* */
-JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
+void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
-    boost::optional<const Ordering&> ordering,
+    const FastVector<VariableSlots::const_iterator>& orderedSlots) {
-    boost::optional<const VariableSlots&> p_variableSlots) {
-  gttic(JacobianFactor_combine_constructor);
-
-  // Compute VariableSlots if one was not provided
-  // Binds reference, does not copy VariableSlots
-  const VariableSlots & variableSlots =
-      p_variableSlots ? p_variableSlots.get() : VariableSlots(graph);
 
   // Cast or convert to Jacobians
   FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
       graph);
 
-  gttic(Order_slots);
-  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
-  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
-  // be added after all of the ordered variables.
-  FastVector<VariableSlots::const_iterator> orderedSlots;
-  orderedSlots.reserve(variableSlots.size());
-  if (ordering) {
-    // If an ordering is provided, arrange the slots first that ordering
-    FastList<VariableSlots::const_iterator> unorderedSlots;
-    size_t nOrderingSlotsUsed = 0;
-    orderedSlots.resize(ordering->size());
-    FastMap<Key, size_t> inverseOrdering = ordering->invert();
-    for (VariableSlots::const_iterator item = variableSlots.begin();
-        item != variableSlots.end(); ++item) {
-      FastMap<Key, size_t>::const_iterator orderingPosition =
-          inverseOrdering.find(item->first);
-      if (orderingPosition == inverseOrdering.end()) {
-        unorderedSlots.push_back(item);
-      } else {
-        orderedSlots[orderingPosition->second] = item;
-        ++nOrderingSlotsUsed;
-      }
-    }
-    if (nOrderingSlotsUsed != ordering->size())
-      throw std::invalid_argument(
-          "The ordering provided to the JacobianFactor combine constructor\n"
-              "contained extra variables that did not appear in the factors to combine.");
-    // Add the remaining slots
-    for(VariableSlots::const_iterator item: unorderedSlots) {
-      orderedSlots.push_back(item);
-    }
-  } else {
-    // If no ordering is provided, arrange the slots as they were, which will be sorted
-    // numerically since VariableSlots uses a map sorting on Key.
-    for (VariableSlots::const_iterator item = variableSlots.begin();
-        item != variableSlots.end(); ++item)
-      orderedSlots.push_back(item);
-  }
-  gttoc(Order_slots);
-
   // Count dimensions
   FastVector<DenseIndex> varDims;
   DenseIndex m, n;
@@ -344,6 +297,127 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
     this->setModel(anyConstrained, *sigmas);
 }
 
+/* ************************************************************************* */
+// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
+// 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
+// be added after all of the ordered variables.
+FastVector<VariableSlots::const_iterator> orderedSlotsHelper(
+    const Ordering& ordering,
+    const VariableSlots& variableSlots) {
+  gttic(Order_slots);
+  
+  FastVector<VariableSlots::const_iterator> orderedSlots;
+  orderedSlots.reserve(variableSlots.size());
+  
+  // If an ordering is provided, arrange the slots first that ordering
+  FastList<VariableSlots::const_iterator> unorderedSlots;
+  size_t nOrderingSlotsUsed = 0;
+  orderedSlots.resize(ordering.size());
+  FastMap<Key, size_t> inverseOrdering = ordering.invert();
+  for (VariableSlots::const_iterator item = variableSlots.begin();
+      item != variableSlots.end(); ++item) {
+    FastMap<Key, size_t>::const_iterator orderingPosition =
+        inverseOrdering.find(item->first);
+    if (orderingPosition == inverseOrdering.end()) {
+      unorderedSlots.push_back(item);
+    } else {
+      orderedSlots[orderingPosition->second] = item;
+      ++nOrderingSlotsUsed;
+    }
+  }
+  if (nOrderingSlotsUsed != ordering.size())
+    throw std::invalid_argument(
+        "The ordering provided to the JacobianFactor combine constructor\n"
+            "contained extra variables that did not appear in the factors to combine.");
+  // Add the remaining slots
+  for(VariableSlots::const_iterator item: unorderedSlots) {
+    orderedSlots.push_back(item);
+  }
+
+  gttoc(Order_slots);
+
+  return orderedSlots;
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph) {
+  gttic(JacobianFactor_combine_constructor);
+
+  // Compute VariableSlots if one was not provided
+  // Binds reference, does not copy VariableSlots
+  const VariableSlots & variableSlots = VariableSlots(graph);
+
+  gttic(Order_slots);
+  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
+  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
+  // be added after all of the ordered variables.
+  FastVector<VariableSlots::const_iterator> orderedSlots;
+  orderedSlots.reserve(variableSlots.size());
+  
+  // If no ordering is provided, arrange the slots as they were, which will be sorted
+  // numerically since VariableSlots uses a map sorting on Key.
+  for (VariableSlots::const_iterator item = variableSlots.begin();
+      item != variableSlots.end(); ++item)
+    orderedSlots.push_back(item);
+  gttoc(Order_slots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
+    const VariableSlots& p_variableSlots) {
+  gttic(JacobianFactor_combine_constructor);
+
+  // Binds reference, does not copy VariableSlots
+  const VariableSlots & variableSlots = p_variableSlots;
+
+  gttic(Order_slots);
+  // Order variable slots - we maintain the vector of ordered slots, as well as keep a list
+  // 'unorderedSlots' of any variables discovered that are not in the ordering.  Those will then
+  // be added after all of the ordered variables.
+  FastVector<VariableSlots::const_iterator> orderedSlots;
+  orderedSlots.reserve(variableSlots.size());
+  
+  // If no ordering is provided, arrange the slots as they were, which will be sorted
+  // numerically since VariableSlots uses a map sorting on Key.
+  for (VariableSlots::const_iterator item = variableSlots.begin();
+      item != variableSlots.end(); ++item)
+    orderedSlots.push_back(item);
+  gttoc(Order_slots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
+    const Ordering& ordering) {
+  gttic(JacobianFactor_combine_constructor);
+  
+  // Compute VariableSlots if one was not provided
+  // Binds reference, does not copy VariableSlots
+  const VariableSlots & variableSlots = VariableSlots(graph);
+
+  // Order variable slots
+  FastVector<VariableSlots::const_iterator> orderedSlots =
+    orderedSlotsHelper(ordering, variableSlots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
+/* ************************************************************************* */
+JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
+    const Ordering& ordering,
+    const VariableSlots& p_variableSlots) {
+  gttic(JacobianFactor_combine_constructor);
+  
+  // Order variable slots
+  FastVector<VariableSlots::const_iterator> orderedSlots =
+    orderedSlotsHelper(ordering, p_variableSlots);
+
+  JacobianFactorHelper(graph, orderedSlots);
+}
+
 /* ************************************************************************* */
 void JacobianFactor::print(const string& s,
     const KeyFormatter& formatter) const {

gtsam/linear/JacobianFactor.h

@@ -22,6 +22,7 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/base/VerticalBlockMatrix.h>
 #include <gtsam/global_includes.h>
+#include <gtsam/inference/VariableSlots.h>
 
 #include <boost/make_shared.hpp>
 
@@ -147,14 +148,37 @@ namespace gtsam {
     JacobianFactor(
       const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
 
+    /**
+     * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
+     * structure computed for \c graph is already available, providing it will reduce the amount of
+     * computation performed. */
+    explicit JacobianFactor(
+      const GaussianFactorGraph& graph);
+
     /**
      * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
      * structure computed for \c graph is already available, providing it will reduce the amount of
      * computation performed. */
     explicit JacobianFactor(
       const GaussianFactorGraph& graph,
-      boost::optional<const Ordering&> ordering = boost::none,
+      const VariableSlots& p_variableSlots);
-      boost::optional<const VariableSlots&> p_variableSlots = boost::none);
+    
+    /**
+     * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
+     * structure computed for \c graph is already available, providing it will reduce the amount of
+     * computation performed. */
+    explicit JacobianFactor(
+      const GaussianFactorGraph& graph,
+      const Ordering& ordering);
+    
+    /**
+     * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
+     * structure computed for \c graph is already available, providing it will reduce the amount of
+     * computation performed. */
+    explicit JacobianFactor(
+      const GaussianFactorGraph& graph,
+      const Ordering& ordering,
+      const VariableSlots& p_variableSlots);
 
     /** Virtual destructor */
     virtual ~JacobianFactor() {}
@@ -356,6 +380,14 @@ namespace gtsam {
 
   private:
 
+    /**
+     * Helper function for public constructors:
+     * Build a dense joint factor from all the factors in a factor graph.  Takes in
+     * ordered variable slots */
+    void JacobianFactorHelper(
+      const GaussianFactorGraph& graph,
+      const FastVector<VariableSlots::const_iterator>& orderedSlots);
+
     /** Unsafe Constructor that creates an uninitialized Jacobian of right size
      *  @param keys in some order
      *  @param diemnsions of the variables in same order

gtsam/linear/RegularHessianFactor.h

@@ -77,11 +77,17 @@ public:
 
   /// Construct from a GaussianFactorGraph
   RegularHessianFactor(const GaussianFactorGraph& factors,
-                       boost::optional<const Scatter&> scatter = boost::none)
+                       const Scatter& scatter)
       : HessianFactor(factors, scatter) {
     checkInvariants();
   }
 
+  /// Construct from a GaussianFactorGraph
+  RegularHessianFactor(const GaussianFactorGraph& factors)
+      : HessianFactor(factors) {
+    checkInvariants();
+  }
+
 private:
 
   /// Check invariants after construction

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -128,17 +128,17 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
                                        const NonlinearOptimizerParams& params) const {
   // solution of linear solver is an update to the linearization point
   VectorValues delta;
-  boost::optional<const Ordering&> optionalOrdering;
+  Ordering ordering;
   if (params.ordering)
-    optionalOrdering.reset(*params.ordering);
+    ordering = *params.ordering;
 
   // Check which solver we are using
   if (params.isMultifrontal()) {
     // Multifrontal QR or Cholesky (decided by params.getEliminationFunction())
-    delta = gfg.optimize(optionalOrdering, params.getEliminationFunction());
+    delta = gfg.optimize(ordering, params.getEliminationFunction());
   } else if (params.isSequential()) {
     // Sequential QR or Cholesky (decided by params.getEliminationFunction())
-    delta = gfg.eliminateSequential(optionalOrdering, params.getEliminationFunction(), boost::none,
+    delta = gfg.eliminateSequential(ordering, params.getEliminationFunction(), boost::none,
                                     params.orderingType)->optimize();
   } else if (params.isIterative()) {
     // Conjugate Gradient -> needs params.iterativeParams