Caught some stragglers using boost::optional<Ordering>

gtsam/discrete/DiscreteConditional.cpp

@@ -46,16 +46,25 @@ DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
 
 /* ******************************************************************************** */
 DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
-    const DecisionTreeFactor& marginal, const boost::optional<Ordering>& orderedKeys) :
+    const DecisionTreeFactor& marginal) :
     BaseFactor(
         ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional(
             joint.size()-marginal.size()) {
   if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
     cout << (firstFrontalKey()) << endl; //TODO Print all keys
-  if (orderedKeys) {
+}
-    keys_.clear();
+
-    keys_.insert(keys_.end(), orderedKeys->begin(), orderedKeys->end());
+/* ******************************************************************************** */
-  }
+DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
+    const DecisionTreeFactor& marginal, const Ordering& orderedKeys) :
+    BaseFactor(
+        ISDEBUG("DiscreteConditional::COUNT") ? joint : joint / marginal), BaseConditional(
+            joint.size()-marginal.size()) {
+  if (ISDEBUG("DiscreteConditional::DiscreteConditional"))
+    cout << (firstFrontalKey()) << endl; //TODO Print all keys
+    
+  keys_.clear();
+  keys_.insert(keys_.end(), orderedKeys.begin(), orderedKeys.end());
 }
 
 /* ******************************************************************************** */

gtsam/discrete/DiscreteConditional.h

@@ -60,7 +60,11 @@ public:
 
   /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */
   DiscreteConditional(const DecisionTreeFactor& joint,
-      const DecisionTreeFactor& marginal, const boost::optional<Ordering>& orderedKeys = boost::none);
+      const DecisionTreeFactor& marginal);
+
+  /** construct P(X|Y)=P(X,Y)/P(Y) from P(X,Y) and P(Y) */
+  DiscreteConditional(const DecisionTreeFactor& joint,
+      const DecisionTreeFactor& marginal, const Ordering& orderedKeys);
 
   /**
    * Combine several conditional into a single one.

gtsam/linear/Scatter.cpp

@@ -34,8 +34,7 @@ string SlotEntry::toString() const {
 }
 
 /* ************************************************************************* */
-void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) {
+void Scatter::setDimensions(const GaussianFactorGraph& gfg, size_t sortStart) {
-  // Now, find dimensions of variables and/or extend
   for (const auto& factor : gfg) {
     if (!factor)
       continue;
@@ -66,7 +65,7 @@ void Scatter::ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart) {
 Scatter::Scatter(const GaussianFactorGraph& gfg) {
   gttic(Scatter_Constructor);
 
-  ScatterHelper(gfg, 0);
+  setDimensions(gfg, 0);
 }
 
 /* ************************************************************************* */
@@ -79,7 +78,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
     add(key, 0);
   }
 
-  ScatterHelper(gfg, ordering.size());
+  setDimensions(gfg, ordering.size());
 }
 
 /* ************************************************************************* */

gtsam/linear/Scatter.h

@@ -64,7 +64,7 @@ class Scatter : public FastVector<SlotEntry> {
 
   /// Helper function for constructors, adds/finds dimensions of variables and
   //  sorts starting from sortStart
-  void ScatterHelper(const GaussianFactorGraph& gfg, size_t sortStart);
+  void setDimensions(const GaussianFactorGraph& gfg, size_t sortStart);
 
   /// Find the SlotEntry with the right key (linear time worst case)
   iterator find(Key key);

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -344,23 +344,31 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
 }
 
 /* ************************************************************************* */
-static Scatter scatterFromValues(const Values& values, boost::optional<Ordering&> ordering) {
+static Scatter scatterFromValues(const Values& values) {
   gttic(scatterFromValues);
 
   Scatter scatter;
   scatter.reserve(values.size());
 
-  if (!ordering) {
+  // use "natural" ordering with keys taken from the initial values
-    // use "natural" ordering with keys taken from the initial values
+  for (const auto& key_value : values) {
-    for (const auto& key_value : values) {
+    scatter.add(key_value.key, key_value.value.dim());
-      scatter.add(key_value.key, key_value.value.dim());
+  }
-    }
+
-  } else {
+  return scatter;
-    // copy ordering into keys and lookup dimension in values, is O(n*log n)
+}
-    for (Key key : *ordering) {
+
-      const Value& value = values.at(key);
+/* ************************************************************************* */
-      scatter.add(key, value.dim());
+static Scatter scatterFromValues(const Values& values, const Ordering& ordering) {
-    }
+  gttic(scatterFromValues);
+
+  Scatter scatter;
+  scatter.reserve(values.size());
+
+  // copy ordering into keys and lookup dimension in values, is O(n*log n)
+  for (Key key : ordering) {
+    const Value& value = values.at(key);
+    scatter.add(key, value.dim());
   }
 
   return scatter;
@@ -368,7 +376,15 @@ static Scatter scatterFromValues(const Values& values, boost::optional<Ordering&
 
 /* ************************************************************************* */
 HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
-    const Values& values, boost::optional<Ordering&> ordering, const Dampen& dampen) const {
+    const Values& values, const Dampen& dampen) const {
+  KeyVector keys = values.keys();
+  Ordering defaultOrdering(keys);
+  return linearizeToHessianFactor(values, defaultOrdering, dampen);
+}
+
+/* ************************************************************************* */
+HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
+    const Values& values, const Ordering& ordering, const Dampen& dampen) const {
   gttic(NonlinearFactorGraph_linearizeToHessianFactor);
 
   Scatter scatter = scatterFromValues(values, ordering);
@@ -395,7 +411,16 @@ HessianFactor::shared_ptr NonlinearFactorGraph::linearizeToHessianFactor(
 
 /* ************************************************************************* */
 Values NonlinearFactorGraph::updateCholesky(const Values& values,
-                                            boost::optional<Ordering&> ordering,
+                                            const Dampen& dampen) const {
+  gttic(NonlinearFactorGraph_updateCholesky);
+  auto hessianFactor = linearizeToHessianFactor(values, dampen);
+  VectorValues delta = hessianFactor->solve();
+  return values.retract(delta);
+}
+
+/* ************************************************************************* */
+Values NonlinearFactorGraph::updateCholesky(const Values& values,
+                                            const Ordering& ordering,
                                             const Dampen& dampen) const {
   gttic(NonlinearFactorGraph_updateCholesky);
   auto hessianFactor = linearizeToHessianFactor(values, ordering, dampen);

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -149,17 +149,31 @@ namespace gtsam {
      * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly
      * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing
      * a new graph, and hence useful in case a dense solve is appropriate for your problem.
-     * An optional ordering can be given that still decides how the Hessian is laid out.
      * An optional lambda function can be used to apply damping on the filled Hessian.
      * No parallelism is exploited, because all the factors write in the same memory.
      */
     boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
-        const Values& values, boost::optional<Ordering&> ordering = boost::none,
+        const Values& values, const Dampen& dampen = nullptr) const;
-        const Dampen& dampen = nullptr) const;
+
+    /**
+     * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly
+     * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing
+     * a new graph, and hence useful in case a dense solve is appropriate for your problem.
+     * An ordering is given that still decides how the Hessian is laid out.
+     * An optional lambda function can be used to apply damping on the filled Hessian.
+     * No parallelism is exploited, because all the factors write in the same memory.
+     */
+    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
+        const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
 
     /// Linearize and solve in one pass.
     /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.
-    Values updateCholesky(const Values& values, boost::optional<Ordering&> ordering = boost::none,
+    Values updateCholesky(const Values& values,
+                          const Dampen& dampen = nullptr) const;
+
+    /// Linearize and solve in one pass.
+    /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.
+    Values updateCholesky(const Values& values, const Ordering& ordering,
                           const Dampen& dampen = nullptr) const;
 
     /// Clone() performs a deep-copy of the graph, including all of the factors

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -82,7 +82,7 @@ public:
   };
 
   LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
-  boost::optional<Ordering> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty)
+  boost::optional<Ordering> ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty)
   IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
 
   inline bool isMultifrontal() const {

gtsam_unstable/nonlinear/NonlinearClusterTree.h

@@ -46,21 +46,26 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
     // linearize local custer factors straight into hessianFactor, which is returned
     // If no ordering given, uses colamd
     HessianFactor::shared_ptr linearizeToHessianFactor(
-        const Values& values, boost::optional<Ordering> ordering = boost::none,
+        const Values& values,
         const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
-      if (!ordering)
+      Ordering ordering;
-        ordering.reset(Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true));
+      ordering = Ordering::ColamdConstrainedFirst(factors, orderedFrontalKeys, true);
-      return factors.linearizeToHessianFactor(values, *ordering, dampen);
+      return factors.linearizeToHessianFactor(values, ordering, dampen);
     }
 
-    // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
+    // linearize local custer factors straight into hessianFactor, which is returned
+    // If no ordering given, uses colamd
+    HessianFactor::shared_ptr linearizeToHessianFactor(
+        const Values& values, const Ordering& ordering,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      return factors.linearizeToHessianFactor(values, ordering, dampen);
+    }
+
+    // Helper function: recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
     // TODO(frank): Use TBB to support deep trees and parallelism
     std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
-        const Values& values, boost::optional<Ordering> ordering = boost::none,
+        const Values& values,
-        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+        const HessianFactor::shared_ptr& localFactor) const {
-      // Linearize and create HessianFactor f(front,separator)
-      HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen);
-
       // Get contributions f(front) from children, as well as p(children|front)
       GaussianBayesNet bayesNet;
       for (const auto& child : children) {
@@ -72,12 +77,45 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
       return {bayesNet, localFactor};
     }
 
+    // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
+    // TODO(frank): Use TBB to support deep trees and parallelism
+    std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
+        const Values& values,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      // Linearize and create HessianFactor f(front,separator)
+      HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, dampen);
+      return linearizeAndEliminate(values, localFactor);
+    }
+
+    // Recursively eliminate subtree rooted at this Cluster into a Bayes net and factor on parent
+    // TODO(frank): Use TBB to support deep trees and parallelism
+    std::pair<GaussianBayesNet, HessianFactor::shared_ptr> linearizeAndEliminate(
+        const Values& values, const Ordering& ordering,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      // Linearize and create HessianFactor f(front,separator)
+      HessianFactor::shared_ptr localFactor = linearizeToHessianFactor(values, ordering, dampen);
+      return linearizeAndEliminate(values, localFactor);
+    }
+
     // Recursively eliminate subtree rooted at this Cluster
     // Version that updates existing Bayes net and returns a new Hessian factor on parent clique
     // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor
     HessianFactor::shared_ptr linearizeAndEliminate(
         const Values& values, GaussianBayesNet* bayesNet,
-        boost::optional<Ordering> ordering = boost::none,
+        const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
+      auto bayesNet_newFactor_pair = linearizeAndEliminate(values, dampen);
+      if (bayesNet) {
+        bayesNet->push_back(bayesNet_newFactor_pair.first);
+      }
+      return bayesNet_newFactor_pair.second;
+    }
+
+    // Recursively eliminate subtree rooted at this Cluster
+    // Version that updates existing Bayes net and returns a new Hessian factor on parent clique
+    // It is possible to pass in a nullptr for the bayesNet if only interested in the new factor
+    HessianFactor::shared_ptr linearizeAndEliminate(
+        const Values& values, GaussianBayesNet* bayesNet,
+        const Ordering& ordering,
         const NonlinearFactorGraph::Dampen& dampen = nullptr) const {
       auto bayesNet_newFactor_pair = linearizeAndEliminate(values, ordering, dampen);
       if (bayesNet) {

tests/testNonlinearFactorGraph.cpp

@@ -198,7 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) {
       }
     }
   };
-  EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6));
+  EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6));
 }
 
 /* ************************************************************************* */