Merge pull request #925 from borglab/fix/889

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -78,29 +78,31 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<class FACTORGRAPH>
+  template <class FACTORGRAPH>
-  boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
+  boost::shared_ptr<
+      typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
   EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
       OptionalOrderingType orderingType, const Eliminate& function,
-    OptionalVariableIndex variableIndex) const
+      OptionalVariableIndex variableIndex) const {
-  {
+    if (!variableIndex) {
-    if(!variableIndex) {
+      // If no VariableIndex provided, compute one and call this function again
-      // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
+      // IMPORTANT: we check for no variable index first so that it's always
-      // for no variable index first so that it's always computed if we need to call COLAMD because
+      // computed if we need to call COLAMD because no Ordering is provided.
-      // no Ordering is provided.  When removing optional from VariableIndex, create VariableIndex
+      // When removing optional from VariableIndex, create VariableIndex before
-      // before creating ordering.
+      // creating ordering.
       VariableIndex computedVariableIndex(asDerived());
-      return eliminateMultifrontal(function, computedVariableIndex, orderingType);
+      return eliminateMultifrontal(orderingType, function,
-    }
+                                   computedVariableIndex);
-    else {
+    } else {
-      // Compute an ordering and call this function again.  We are guaranteed to have a
+      // Compute an ordering and call this function again.  We are guaranteed to
-      // VariableIndex already here because we computed one if needed in the previous 'if' block.
+      // have a VariableIndex already here because we computed one if needed in
+      // the previous 'if' block.
       if (orderingType == Ordering::METIS) {
         Ordering computedOrdering = Ordering::Metis(asDerived());
-        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
+        return eliminateMultifrontal(computedOrdering, function, variableIndex);
       } else {
         Ordering computedOrdering = Ordering::Colamd(*variableIndex);
-        return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
+        return eliminateMultifrontal(computedOrdering, function, variableIndex);
       }
     }
   }
@@ -273,7 +275,7 @@ namespace gtsam {
       else
       {
         // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
-        return factorGraph->eliminateSequential(function);
+        return factorGraph->eliminateSequential(Ordering::COLAMD, function);
       }
     }
   }
@@ -340,7 +342,7 @@ namespace gtsam {
       else
       {
         // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
-        return factorGraph->eliminateMultifrontal(function);
+        return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
       }
     }
   }

gtsam/inference/EliminateableFactorGraph.h

@@ -288,6 +288,7 @@ namespace gtsam {
     FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
 
   public:
+  #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
     /** \deprecated ordering and orderingType shouldn't both be specified */
     boost::shared_ptr<BayesNetType> eliminateSequential(
       const Ordering& ordering,
@@ -339,6 +340,7 @@ namespace gtsam {
       OptionalVariableIndex variableIndex = boost::none) const {
           return marginalMultifrontalBayesTree(variables, function, variableIndex);
       }
+  #endif
   };
 
 }

gtsam/linear/GaussianFactorGraph.cpp

@@ -19,7 +19,6 @@
  */
 
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
@@ -290,10 +289,11 @@ namespace gtsam {
     return blocks;
   }
 
-  /* ************************************************************************* */
+  /* ************************************************************************ */
   VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const {
     gttic(GaussianFactorGraph_optimize);
-    return BaseEliminateable::eliminateMultifrontal(function)->optimize();
+    return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function)
+        ->optimize();
   }
 
   /* ************************************************************************* */
@@ -503,13 +503,6 @@ namespace gtsam {
     return e;
   }
 
-  /* ************************************************************************* */
-  /** \deprecated */
-  VectorValues GaussianFactorGraph::optimize(boost::none_t,
-    const Eliminate& function) const {
-      return optimize(function);
-  }
-
   /* ************************************************************************* */
   void GaussianFactorGraph::printErrors(
       const VectorValues& values, const std::string& str,

gtsam/linear/GaussianFactorGraph.h

@@ -21,12 +21,13 @@
 
 #pragma once
 
-#include <gtsam/inference/FactorGraph.h>
 #include <gtsam/inference/EliminateableFactorGraph.h>
-#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/inference/FactorGraph.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/Errors.h>  // Included here instead of fw-declared so we can use Errors::iterator
+#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/linear/HessianFactor.h>
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/VectorValues.h>
 
 namespace gtsam {
 
@@ -395,9 +396,14 @@ namespace gtsam {
 
   public:
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
    /** \deprecated */
    VectorValues optimize(boost::none_t,
-      const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
+                         const Eliminate& function =
+                             EliminationTraitsType::DefaultEliminate) const {
+     return optimize(function);
+   }
+#endif
 
   };
 

gtsam/nonlinear/Marginals.cpp

@@ -80,11 +80,15 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut
 
 /* ************************************************************************* */
 void Marginals::computeBayesTree() {
+  // The default ordering to use.
+  const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD;
   // Compute BayesTree
-  if(factorization_ == CHOLESKY)
+  if (factorization_ == CHOLESKY)
-    bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky);
+    bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType,
-  else if(factorization_ == QR)
+                                               EliminatePreferCholesky);
-    bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR);
+  else if (factorization_ == QR)
+    bayesTree_ =
+        *graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR);
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/Marginals.h

@@ -131,6 +131,7 @@ protected:
   void computeBayesTree(const Ordering& ordering);
 
 public:
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   /** \deprecated argument order changed due to removing boost::optional<Ordering> */
   Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
             const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
@@ -142,6 +143,7 @@ public:
   /** \deprecated argument order changed due to removing boost::optional<Ordering> */
   Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
             const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
+#endif
 
 };
 

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -265,6 +265,7 @@ namespace gtsam {
 
   public:
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
     /** \deprecated */
     boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
         const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
@@ -274,6 +275,7 @@ namespace gtsam {
     Values updateCholesky(const Values& values, boost::none_t,
                           const Dampen& dampen = nullptr) const
       {return updateCholesky(values, dampen);}
+#endif
 
   };
 

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -147,11 +147,13 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
   } else if (params.isSequential()) {
     // Sequential QR or Cholesky (decided by params.getEliminationFunction())
     if (params.ordering)
-      delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
+      delta = gfg.eliminateSequential(*params.ordering,
-                                      boost::none, params.orderingType)->optimize();
+                                      params.getEliminationFunction())
+                  ->optimize();
     else
-      delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none,
+      delta = gfg.eliminateSequential(params.orderingType,
-                                      params.orderingType)->optimize();
+                                      params.getEliminationFunction())
+                  ->optimize();
   } else if (params.isIterative()) {
     // Conjugate Gradient -> needs params.iterativeParams
     if (!params.iterativeParams)