Fixed errors and warnings in new unordered code

gtsam/base/VerticalBlockMatrix.cpp

@@ -29,6 +29,7 @@ namespace gtsam {
     result.matrix_.resize(rhs.rows(), result.variableColOffsets_.back());
     result.rowEnd_ = rhs.rows();
     result.assertInvariants();
+    return result;
   }
 
 }

gtsam/base/VerticalBlockMatrix.h

@@ -70,11 +70,11 @@ namespace gtsam {
      * Construct from a container of the sizes of each vertical block, resize the matrix so that its
      * height is matrixNewHeight and its width fits the given block dimensions. */
     template<typename CONTAINER>
-    VerticalBlockMatrix(const CONTAINER dimensions, int height) :
+    VerticalBlockMatrix(const CONTAINER dimensions, DenseIndex height) :
-      matrix_(matrixNewHeight, variableColOffsets_.back()),
+      matrix_(height, variableColOffsets_.back()),
-      rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0)
+      rowStart_(0), rowEnd_(height), blockStart_(0)
     {
-      fillOffsets(firstBlockDim, lastBlockDim);
+      fillOffsets(dimensions.begin(), dimensions.end());
       assertInvariants();
     }
 
@@ -82,9 +82,9 @@ namespace gtsam {
      * Construct from iterator over the sizes of each vertical block, resize the matrix so that its
      * height is matrixNewHeight and its width fits the given block dimensions. */
     template<typename ITERATOR>
-    VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, int height) :
+    VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) :
-      matrix_(matrixNewHeight, variableColOffsets_.back()),
+      matrix_(height, variableColOffsets_.back()),
-      rowStart_(0), rowEnd_(matrixNewHeight), blockStart_(0)
+      rowStart_(0), rowEnd_(height), blockStart_(0)
     {
       fillOffsets(firstBlockDim, lastBlockDim);
       assertInvariants();
@@ -182,15 +182,15 @@ namespace gtsam {
   protected:
     void assertInvariants() const {
       assert(matrix_.cols() == variableColOffsets_.back());
-      assert(blockStart_ < variableColOffsets_.size());
+      assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
       assert(rowStart_ <= matrix_.rows());
       assert(rowEnd_ <= matrix_.rows());
       assert(rowStart_ <= rowEnd_);
     }
 
-    void checkBlock(Index block) const {
+    void checkBlock(DenseIndex block) const {
       assert(matrix_.cols() == variableColOffsets_.back());
-      assert(block < variableColOffsets_.size()-1);
+      assert(block < (DenseIndex)variableColOffsets_.size()-1);
       assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
     }
 

gtsam/inference/ConditionalUnordered.h

@@ -35,28 +35,22 @@ namespace gtsam {
    * \nosubgrouping
    */
   template<class FACTOR, class DERIVEDCONDITIONAL>
-  class ConditionalUnordered {
+  class ConditionalUnordered
-
+  {
   protected:
-
     /** The first nrFrontal variables are frontal and the rest are parents. */
     size_t nrFrontals_;
 
-    /** Iterator over keys */
+  private:
-    using typename FACTOR::iterator; // 'using' instead of typedef to avoid ambiguous symbol from multiple inheritance
+    /// Typedef to this class
-
-    /** Const iterator over keys */
-    using typename FACTOR::const_iterator; // 'using' instead of typedef to avoid ambiguous symbol from multiple inheritance
-
-  public:
-
     typedef ConditionalUnordered<FACTOR,DERIVEDCONDITIONAL> This;
 
+  public:
     /** View of the frontal keys (call frontals()) */
-    typedef boost::iterator_range<const_iterator> Frontals;
+    typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
 
     /** View of the separator keys (call parents()) */
-    typedef boost::iterator_range<const_iterator> Parents;
+    typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
 
     /// @name Standard Constructors
     /// @{
@@ -103,34 +97,32 @@ namespace gtsam {
     Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
 
     /** Iterator pointing to first frontal key. */
-    const_iterator beginFrontals() const { return asFactor().begin(); }
+    typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
     
     /** Iterator pointing past the last frontal key. */
-    const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
+    typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
     
     /** Iterator pointing to the first parent key. */
-    const_iterator beginParents() const { return endFrontals(); }
+    typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
 
     /** Iterator pointing past the last parent key. */
-    const_iterator endParents() const { return asFactor().end(); }
+    typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
 
     /// @}
     /// @name Advanced Interface
     /// @{
 
-    /** Mutable iterators and accessors */
+    /** Mutable iterator pointing to first frontal key. */
-    iterator beginFrontals() {
+    typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
-      return asFactor().begin();
+
-    } ///<TODO: comment
+    /** Mutable iterator pointing past the last frontal key. */
-    iterator endFrontals() {
+    typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
-      return asFactor().begin() + nrFrontals_;
+
-    } ///<TODO: comment
+    /** Mutable iterator pointing to the first parent key. */
-    iterator beginParents() {
+    typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
-      return asFactor().begin() + nrFrontals_;
+
-    } ///<TODO: comment
+    /** Mutable iterator pointing past the last parent key. */
-    iterator endParents() {
+    typename FACTOR::iterator endParents() { return asFactor().end(); }
-      return asFactor().end();
-    } ///<TODO: comment
 
   private:
     // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -31,8 +31,8 @@ namespace gtsam {
   {
     if(ordering && variableIndex) {
       // Do elimination
-      std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> > result
+      std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > result
-        = ELIMINATIONTREE(asDerived(), *variableIndex, *ordering).eliminate(function);
+        = EliminationTreeType(asDerived(), *variableIndex, *ordering).eliminate(function);
       // If any factors are remaining, the ordering was incomplete
       if(!result.second->empty())
         throw InconsistentEliminationRequested();
@@ -61,8 +61,8 @@ namespace gtsam {
   {
     if(ordering && variableIndex) {
       // Do elimination with given ordering
-      std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> > result
+      std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > result
-        = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, *ordering)).eliminate(function);
+        = JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, *ordering)).eliminate(function);
       // If any factors are remaining, the ordering was incomplete
       if(!result.second->empty())
         throw InconsistentEliminationRequested();
@@ -87,14 +87,14 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
     EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
-    const Eliminate& function, const OrderingUnordered& ordering, OptionalVariableIndex variableIndex) const
+    const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex) {
       // Do elimination
-      return ELIMINATIONTREE(asDerived(), *variableIndex, ordering).eliminate(function);
+      return EliminationTreeType(asDerived(), *variableIndex, ordering).eliminate(function);
     } else {
       // If no variable index is provided, compute one and call this function again
-      return eliminatePartialSequential(function, ordering, VariableIndexUnordered(asDerived()));
+      return eliminatePartialSequential(ordering, function, VariableIndexUnordered(asDerived()));
     }
   }
 
@@ -102,14 +102,14 @@ namespace gtsam {
   template<class FACTORGRAPH>
   std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
     EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
-    const Eliminate& function, const OrderingUnordered& ordering, OptionalVariableIndex variableIndex) const
+    const OrderingUnordered& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
   {
     if(variableIndex) {
       // Do elimination
-      return JUNCTIONTREE(ELIMINATIONTREE(asDerived(), *variableIndex, ordering)).eliminate(function);
+      return JunctionTreeType(EliminationTreeType(asDerived(), *variableIndex, ordering)).eliminate(function);
     } else {
       // If no variable index is provided, compute one and call this function again
-      return eliminatePartialMultifrontal(function, ordering, VariableIndexUnordered(asDerived()));
+      return eliminatePartialMultifrontal(ordering, function, VariableIndexUnordered(asDerived()));
     }
   }
 

gtsam/inference/EliminateableFactorGraph.h

@@ -30,10 +30,11 @@ namespace gtsam {
   /// elimination, etc.  This must be defined for each factor graph that inherits from
   /// EliminateableFactorGraph.
   template<class GRAPH>
-  class EliminationTraits
+  struct EliminationTraits
   {
     // Template for deriving:
     // typedef MyFactor FactorType;                   ///< Type of factors in factor graph (e.g. GaussianFactor)
+    // typedef MyFactorGraphType FactorGraphType;     ///< Type of the factor graph (e.g. GaussianFactorGraph)
     // typedef MyConditional ConditionalType;         ///< Type of conditionals from elimination (e.g. GaussianConditional)
     // typedef MyBayesNet BayesNetType;               ///< Type of Bayes net from sequential elimination (e.g. GaussianBayesNet)
     // typedef MyEliminationTree EliminationTreeType; ///< Type of elimination tree (e.g. GaussianEliminationTree)
@@ -41,8 +42,7 @@ namespace gtsam {
     // typedef MyJunctionTree JunctionTreeType;       ///< Type of Junction tree (e.g. GaussianJunctionTree)
     // static pair<shared_ptr<ConditionalType>, shared_ptr<FactorType>
     //   DefaultEliminate(
-    //   const std::vector<boost::shared_ptr<FactorType> >& factors,
+    //   const MyFactorGraph& factors, const OrderingUnordered& keys); ///< The default dense elimination function
-    //   const std::vector<Key>& keys); ///< The default dense elimination function
   };
 
 
@@ -55,12 +55,12 @@ namespace gtsam {
   {
   private:
     typedef EliminateableFactorGraph<FACTORGRAPH> This; ///< Typedef to this class.
-    typedef FACTORGRAPH FactorGraphType;
+    typedef FACTORGRAPH FactorGraphType; ///< Typedef to factor graph type
+    // Base factor type stored in this graph (private because derived classes will get this from
+    // their FactorGraph base class)
+    typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType;
 
   public:
-    /// Base factor type stored in this graph
-    typedef typename EliminationTraits<FactorGraphType>::FactorType FactorType;
-
     /// Conditional type stored in the Bayes net produced by elimination
     typedef typename EliminationTraits<FactorGraphType>::ConditionalType ConditionalType;
 
@@ -78,10 +78,10 @@ namespace gtsam {
 
     /// The pair of conditional and remaining factor produced by a single dense elimination step on
     /// a subgraph.
-    typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > EliminationResult;
+    typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
 
     /// The function type that does a single dense elimination step on a subgraph.
-    typedef boost::function<EliminationResult(std::vector<boost::shared_ptr<FactorType> >, std::vector<Key>)> Eliminate;
+    typedef boost::function<EliminationResult(std::vector<boost::shared_ptr<_FactorType> >, std::vector<Key>)> Eliminate;
 
     /// Typedef for an optional ordering as an argument to elimination functions
     typedef boost::optional<const OrderingUnordered&> OptionalOrdering;

gtsam/inference/EliminationTreeUnordered-inst.h

@@ -137,6 +137,7 @@ namespace gtsam {
     } catch(std::invalid_argument& e) {
       // If this is thrown from structure[order[j]] above, it means that it was requested to
       // eliminate a variable not present in the graph, so throw a more informative error message.
+      (void)e; // Prevent unused variable warning
       throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph");
     } catch(...) {
       throw;

gtsam/inference/FactorGraphUnordered.h

@@ -43,7 +43,6 @@ namespace gtsam {
   public:
     typedef FACTOR FactorType;  ///< factor type
     typedef boost::shared_ptr<FACTOR> sharedFactor;  ///< Shared pointer to a factor
-    typedef boost::shared_ptr<typename FACTOR::ConditionalType> sharedConditional;  ///< Shared pointer to a conditional
     typedef typename std::vector<sharedFactor>::iterator iterator;
     typedef typename std::vector<sharedFactor>::const_iterator const_iterator;
 

gtsam/inference/JunctionTreeUnordered-inst.h

@@ -73,17 +73,17 @@ namespace gtsam {
       // current node did not introduce any parents beyond those already in the child.
 
       // Do symbolic elimination for this node
-      std::vector<SymbolicFactorUnordered::shared_ptr> symbolicFactors;
+      SymbolicFactorGraphUnordered symbolicFactors;
       symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size());
       // Add symbolic versions of the ETree node factors
       BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) {
         symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(
           SymbolicFactorUnordered::FromKeys(*factor))); }
       // Add symbolic factors passed up from children
-      symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
+      symbolicFactors.push_back(myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
-      std::vector<Key> keyAsVector(1); keyAsVector[0] = ETreeNode->key;
+      OrderingUnordered keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key);
       std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult =
-        EliminateSymbolicUnordered(symbolicFactors, keyAsVector);
+        EliminateSymbolicUnordered(symbolicFactors, keyAsOrdering);
 
       // Store symbolic elimination results in the parent
       myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);

gtsam/linear/GaussianConditionalUnordered-inl.h

@@ -38,14 +38,14 @@ namespace gtsam {
   BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {}
 
   /* ************************************************************************* */
-  template<typename KEYS, class MATRIX>
+  template<typename KEYS>
   GaussianConditionalUnordered::GaussianConditionalUnordered(
     const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) :
   BaseFactor(keys, augmentedMatrix, sigmas), BaseConditional(nrFrontals) {}
 
   /* ************************************************************************* */
   template<typename ITERATOR>
-  GaussianConditional::shared_ptr GaussianConditional::Combine(ITERATOR firstConditional, ITERATOR lastConditional)
+  GaussianConditionalUnordered::shared_ptr GaussianConditionalUnordered::Combine(ITERATOR firstConditional, ITERATOR lastConditional)
   {
     // TODO:  check for being a clique
 

gtsam/linear/GaussianConditionalUnordered.cpp

@@ -75,7 +75,7 @@ namespace gtsam {
       return false;
 
     // check if R_ and d_ are linear independent
-    for (size_t i=0; i<Ab_.rows(); i++) {
+    for (DenseIndex i=0; i<Ab_.rows(); i++) {
       list<Vector> rows1; rows1.push_back(Vector(get_R().row(i)));
       list<Vector> rows2; rows2.push_back(Vector(c.get_R().row(i)));
 

gtsam/linear/GaussianConditionalUnordered.h

@@ -81,7 +81,7 @@ namespace gtsam {
      *  factor. */
     template<typename KEYS>
     GaussianConditionalUnordered(
-      const KEYS& keys, size_t nrFrontals const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas);
+      const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas);
 
     /** Combine several GaussianConditional into a single dense GC.  The conditionals enumerated by
     *   \c first and \c last must be in increasing order, meaning that the parents of any

gtsam/linear/GaussianFactorGraphUnordered.cpp

@@ -19,6 +19,8 @@
  */
 
 #include <gtsam/linear/GaussianFactorGraphUnordered.h>
+#include <gtsam/linear/VectorValuesUnordered.h>
+#include <gtsam/linear/GaussianBayesTreeUnordered.h>
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>
 #include <gtsam/base/cholesky.h>
@@ -28,6 +30,12 @@ using namespace gtsam;
 
 namespace gtsam {
 
+  /* ************************************************************************* */
+  void GaussianFactorGraphUnordered::push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree)
+  {
+    Base::push_back_bayesTree(bayesTree);
+  }
+
   /* ************************************************************************* */
   GaussianFactorGraphUnordered::Keys GaussianFactorGraphUnordered::keys() const {
     FastSet<Key> keys;
@@ -143,130 +151,58 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  GaussianFactorGraphUnordered::EliminationResult EliminateJacobians(const FactorGraph<
+  //Matrix GaussianFactorGraphUnordered::augmentedHessian() const {
-      JacobianFactorUnordered>& factors, size_t nrFrontals) {
+  //  // combine all factors and get upper-triangular part of Hessian
-  }
+  //  HessianFactorUnordered combined(*this);
+  //  Matrix result = combined.info();
+  //  // Fill in lower-triangular part of Hessian
+  //  result.triangularView<Eigen::StrictlyLower>() = result.transpose();
+  //  return result;
+  //}
 
   /* ************************************************************************* */
-  Matrix GaussianFactorGraphUnordered::augmentedHessian() const {
+  //std::pair<Matrix,Vector> GaussianFactorGraphUnordered::hessian() const {
-    // combine all factors and get upper-triangular part of Hessian
+  //  Matrix augmented = augmentedHessian();
-    HessianFactorUnordered combined(*this);
+  //  return make_pair(
-    Matrix result = combined.info();
+  //    augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
-    // Fill in lower-triangular part of Hessian
+  //    augmented.col(augmented.rows()-1).head(augmented.rows()-1));
-    result.triangularView<Eigen::StrictlyLower>() = result.transpose();
+  //}
-    return result;
-  }
 
   /* ************************************************************************* */
-  std::pair<Matrix,Vector> GaussianFactorGraphUnordered::hessian() const {
+  //GaussianFactorGraphUnordered::EliminationResult EliminateCholesky(const FactorGraph<
-    Matrix augmented = augmentedHessian();
+  //    GaussianFactorUnordered>& factors, size_t nrFrontals) {
-    return make_pair(
+
-      augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
+  //  const bool debug = ISDEBUG("EliminateCholesky");
-      augmented.col(augmented.rows()-1).head(augmented.rows()-1));
+
-  }
+  //  // Form Ab' * Ab
+  //  gttic(combine);
+  //  HessianFactorUnordered::shared_ptr combinedFactor(new HessianFactorUnordered(factors));
+  //  gttoc(combine);
+
+  //  // Do Cholesky, note that after this, the lower triangle still contains
+  //  // some untouched non-zeros that should be zero.  We zero them while
+  //  // extracting submatrices next.
+  //  gttic(partial_Cholesky);
+  //  combinedFactor->partialCholesky(nrFrontals);
+
+  //  gttoc(partial_Cholesky);
+
+  //  // Extract conditional and fill in details of the remaining factor
+  //  gttic(split);
+  //  GaussianConditional::shared_ptr conditional =
+  //      combinedFactor->splitEliminatedFactor(nrFrontals);
+  //  if (debug) {
+  //    conditional->print("Extracted conditional: ");
+  //    combinedFactor->print("Eliminated factor (L piece): ");
+  //  }
+  //  gttoc(split);
+
+  //  combinedFactor->assertInvariants();
+  //  return make_pair(conditional, combinedFactor);
+  //}
 
   /* ************************************************************************* */
-  GaussianFactorGraphUnordered::EliminationResult EliminateCholesky(const FactorGraph<
+  bool hasConstraints(const GaussianFactorGraphUnordered& factors) {
-      GaussianFactorUnordered>& factors, size_t nrFrontals) {
-
-    const bool debug = ISDEBUG("EliminateCholesky");
-
-    // Form Ab' * Ab
-    gttic(combine);
-    HessianFactorUnordered::shared_ptr combinedFactor(new HessianFactorUnordered(factors));
-    gttoc(combine);
-
-    // Do Cholesky, note that after this, the lower triangle still contains
-    // some untouched non-zeros that should be zero.  We zero them while
-    // extracting submatrices next.
-    gttic(partial_Cholesky);
-    combinedFactor->partialCholesky(nrFrontals);
-
-    gttoc(partial_Cholesky);
-
-    // Extract conditional and fill in details of the remaining factor
-    gttic(split);
-    GaussianConditional::shared_ptr conditional =
-        combinedFactor->splitEliminatedFactor(nrFrontals);
-    if (debug) {
-      conditional->print("Extracted conditional: ");
-      combinedFactor->print("Eliminated factor (L piece): ");
-    }
-    gttoc(split);
-
-    combinedFactor->assertInvariants();
-    return make_pair(conditional, combinedFactor);
-  }
-
-  /* ************************************************************************* */
-  static FactorGraph<JacobianFactorUnordered> convertToJacobians(const FactorGraph<
-      GaussianFactorUnordered>& factors) {
-
-    typedef JacobianFactorUnordered J;
-    typedef HessianFactorUnordered H;
-
-    const bool debug = ISDEBUG("convertToJacobians");
-
-    FactorGraph<J> jacobians;
-    jacobians.reserve(factors.size());
-    BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors)
-    if (factor) {
-      J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
-      if (jacobian) {
-        jacobians.push_back(jacobian);
-        if (debug) jacobian->print("Existing JacobianFactor: ");
-      } else {
-        H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor));
-        if (!hessian) throw std::invalid_argument(
-            "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor.");
-        J::shared_ptr converted(new J(*hessian));
-        if (debug) {
-          cout << "Converted HessianFactor to JacobianFactor:\n";
-          hessian->print("HessianFactor: ");
-          converted->print("JacobianFactor: ");
-          if (!assert_equal(*hessian, HessianFactorUnordered(*converted), 1e-3)) throw runtime_error(
-              "convertToJacobians: Conversion between Jacobian and Hessian incorrect");
-        }
-        jacobians.push_back(converted);
-      }
-    }
-    return jacobians;
-  }
-
-  /* ************************************************************************* */
-  GaussianFactorGraphUnordered::EliminationResult EliminateQR(
-    const std::vector<GaussianFactorUnordered::shared_ptr>& factors, const std::vector<Key>& keys)
-  {
-
-    gttic(Combine);
-    JacobianFactorUnordered jointFactor(factors);
-    gttoc(Combine);
-    gttic(eliminate);
-    GaussianConditional::shared_ptr gbn = jointFactor->eliminate(nrFrontals);
-    gttoc(eliminate);
-    return make_pair(gbn, jointFactor);
-
-
-    const bool debug = ISDEBUG("EliminateQR");
-
-    // Convert all factors to the appropriate type and call the type-specific EliminateGaussian.
-    if (debug) cout << "Using QR" << endl;
-
-    gttic(convert_to_Jacobian);
-    FactorGraph<JacobianFactorUnordered> jacobians = convertToJacobians(factors);
-    gttoc(convert_to_Jacobian);
-
-    gttic(Jacobian_EliminateGaussian);
-    GaussianConditional::shared_ptr conditional;
-    GaussianFactorUnordered::shared_ptr factor;
-    boost::tie(conditional, factor) = EliminateJacobians(jacobians, nrFrontals);
-    gttoc(Jacobian_EliminateGaussian);
-
-    return make_pair(conditional, factor);
-  } // \EliminateQR
-
-  /* ************************************************************************* */
-  bool hasConstraints(const FactorGraph<GaussianFactorUnordered>& factors) {
     typedef JacobianFactorUnordered J;
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& factor, factors) {
       J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
@@ -278,38 +214,40 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  GaussianFactorGraphUnordered::EliminationResult EliminatePreferCholesky(
+  //GaussianFactorGraphUnordered::EliminationResult EliminatePreferCholesky(
-      const FactorGraph<GaussianFactorUnordered>& factors, size_t nrFrontals) {
+  //    const FactorGraph<GaussianFactorUnordered>& factors, size_t nrFrontals) {
 
-    // If any JacobianFactors have constrained noise models, we have to convert
+  //  // If any JacobianFactors have constrained noise models, we have to convert
-    // all factors to JacobianFactors.  Otherwise, we can convert all factors
+  //  // all factors to JacobianFactors.  Otherwise, we can convert all factors
-    // to HessianFactors.  This is because QR can handle constrained noise
+  //  // to HessianFactors.  This is because QR can handle constrained noise
-    // models but Cholesky cannot.
+  //  // models but Cholesky cannot.
-    if (hasConstraints(factors))
+  //  if (hasConstraints(factors))
-      return EliminateQR(factors, nrFrontals);
+  //    return EliminateQR(factors, nrFrontals);
-    else {
+  //  else {
-      GaussianFactorGraphUnordered::EliminationResult ret;
+  //    GaussianFactorGraphUnordered::EliminationResult ret;
-      gttic(EliminateCholesky);
+  //    gttic(EliminateCholesky);
-      ret = EliminateCholesky(factors, nrFrontals);
+  //    ret = EliminateCholesky(factors, nrFrontals);
-      gttoc(EliminateCholesky);
+  //    gttoc(EliminateCholesky);
-      return ret;
+  //    return ret;
-    }
+  //  }
 
-  } // \EliminatePreferCholesky
+  //} // \EliminatePreferCholesky
 
 
 
   /* ************************************************************************* */
-  static JacobianFactorUnordered::shared_ptr convertToJacobianFactorPtr(const GaussianFactorUnordered::shared_ptr &gf) {
+  namespace {
+    JacobianFactorUnordered::shared_ptr convertToJacobianFactorPtr(const GaussianFactorUnordered::shared_ptr &gf) {
       JacobianFactorUnordered::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactorUnordered>(gf);
       if( !result ) {
         result = boost::make_shared<JacobianFactorUnordered>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
       }
       return result;
     }
+  }
 
   /* ************************************************************************* */
-  Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValues& x) {
+  Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x) {
     Errors e;
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
       JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
@@ -319,12 +257,12 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValues& x, Errors& e) {
+  void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, Errors& e) {
     multiplyInPlace(fg,x,e.begin());
   }
 
   /* ************************************************************************* */
-  void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValues& x, const Errors::iterator& e) {
+  void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, const Errors::iterator& e) {
     Errors::iterator ei = e;
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
       JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
@@ -335,7 +273,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   // x += alpha*A'*e
-  void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValues& x) {
+  void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x) {
     // For each factor add the gradient contribution
     Errors::const_iterator ei = e.begin();
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
@@ -345,9 +283,9 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  VectorValues gradient(const GaussianFactorGraphUnordered& fg, const VectorValues& x0) {
+  VectorValuesUnordered gradient(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x0) {
     // It is crucial for performance to make a zero-valued clone of x
-    VectorValues g = VectorValues::Zero(x0);
+    VectorValuesUnordered g = VectorValuesUnordered::Zero(x0);
     Errors e;
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
       JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
@@ -358,7 +296,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValues& g) {
+  void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValuesUnordered& g) {
     // Zero-out the gradient
     g.setZero();
     Errors e;
@@ -370,20 +308,20 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void residual(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) {
+  void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r) {
     Key i = 0 ;
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
       JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
       r[i] = Ai->getb();
       i++;
     }
-    VectorValues Ax = VectorValues::SameStructure(r);
+    VectorValuesUnordered Ax = VectorValuesUnordered::SameStructure(r);
     multiply(fg,x,Ax);
     axpy(-1.0,Ax,r);
   }
 
   /* ************************************************************************* */
-  void multiply(const GaussianFactorGraphUnordered& fg, const VectorValues &x, VectorValues &r) {
+  void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r) {
     r.setZero();
     Key i = 0;
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
@@ -397,7 +335,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValues &r, VectorValues &x) {
+  void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &r, VectorValuesUnordered &x) {
     x.setZero();
     Key i = 0;
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
@@ -410,7 +348,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraphUnordered& fg, const VectorValues& x) {
+  boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x) {
     boost::shared_ptr<Errors> e(new Errors);
     BOOST_FOREACH(const GaussianFactorUnordered::shared_ptr& Ai_G, fg) {
       JacobianFactorUnordered::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);

gtsam/linear/GaussianFactorGraphUnordered.h

@@ -25,6 +25,7 @@
 #include <gtsam/inference/EliminateableFactorGraph.h>
 #include <gtsam/linear/GaussianFactorUnordered.h>
 #include <gtsam/linear/JacobianFactorUnordered.h>
+#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
 
 namespace gtsam {
 
@@ -37,16 +38,11 @@ namespace gtsam {
   class GaussianBayesTreeUnordered;
   class GaussianJunctionTreeUnordered;
 
-  // Forward declaration to use as default elimination function, documented declaration below.
-  GTSAM_EXPORT
-    std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<GaussianFactorUnordered> >
-    EliminateQRUnordered(
-    const std::vector<boost::shared_ptr<GaussianFactorUnordered> >& factors, const std::vector<Key>& keys);
-
   /* ************************************************************************* */
-  template<> class EliminationTraits<GaussianFactorGraphUnordered>
+  template<> struct EliminationTraits<GaussianFactorGraphUnordered>
   {
     typedef GaussianFactorUnordered FactorType;                   ///< Type of factors in factor graph
+    typedef GaussianFactorGraphUnordered FactorGraphType;         ///< Type of the factor graph (e.g. GaussianFactorGraph)
     typedef GaussianConditionalUnordered ConditionalType;         ///< Type of conditionals from elimination
     typedef GaussianBayesNetUnordered BayesNetType;               ///< Type of Bayes net from sequential elimination
     typedef GaussianEliminationTreeUnordered EliminationTreeType; ///< Type of elimination tree
@@ -54,8 +50,8 @@ namespace gtsam {
     typedef GaussianJunctionTreeUnordered JunctionTreeType;       ///< Type of Junction tree
     /// The default dense elimination function
     static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
-      DefaultEliminate(const std::vector<boost::shared_ptr<FactorType> >& factors,
+      DefaultEliminate(const FactorGraphType& factors, const OrderingUnordered& keys) {
-      const std::vector<Key>& keys) { return EliminateQRUnordered(factors, keys); }
+        return EliminateQRUnordered(factors, keys); }
   };
 
   /* ************************************************************************* */
@@ -120,6 +116,9 @@ namespace gtsam {
     void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model) {
       add(JacobianFactorUnordered(terms,b,model)); }
 
+    /** push back a BayesTree as a collection of factors. */
+    void push_back_bayesTree(const GaussianBayesTreeUnordered& bayesTree);
+
     /**
      * Return the set of variables involved in the factors (computes a set
      * union).
@@ -191,7 +190,7 @@ namespace gtsam {
      and the negative log-likelihood is
      \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
      */
-    Matrix augmentedHessian() const;
+    //Matrix augmentedHessian() const;
 
     /**
      * Return the dense Hessian \f$ \Lambda \f$ and information vector
@@ -199,7 +198,7 @@ namespace gtsam {
      * is \frac{1}{2} x^T \Lambda x + \eta^T x + c.  See also
      * GaussianFactorGraph::augmentedHessian.
      */
-    std::pair<Matrix,Vector> hessian() const;
+    //std::pair<Matrix,Vector> hessian() const;
 
   private:
     /** Serialization function */
@@ -211,48 +210,11 @@ namespace gtsam {
 
   };
 
-  /**
-   * Combine and eliminate several factors.
-   * \addtogroup LinearSolving
-   */
-  GTSAM_EXPORT JacobianFactor::shared_ptr CombineJacobians(
-      const FactorGraph<JacobianFactor>& factors,
-      const VariableSlots& variableSlots);
-
   /**
    * Evaluates whether linear factors have any constrained noise models
    * @return true if any factor is constrained.
    */
-  GTSAM_EXPORT bool hasConstraints(const FactorGraph<GaussianFactor>& factors);
+  GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraphUnordered& factors);
-
-  /**
-   * Densely combine and partially eliminate JacobianFactors to produce a
-   * single conditional with nrFrontals frontal variables and a remaining
-   * factor.
-   * Variables are eliminated in the natural order of the variable indices of in the factors.
-   * @param factors Factors to combine and eliminate
-   * @param nrFrontals Number of frontal variables to eliminate.
-   * @return The conditional and remaining factor
-
-   * \addtogroup LinearSolving
-   */
-  GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateJacobians(const FactorGraph<
-      JacobianFactor>& factors, size_t nrFrontals = 1);
-
-  /**
-   * Densely partially eliminate with QR factorization.  HessianFactors are
-   * first factored with Cholesky decomposition to produce JacobianFactors,
-   * by calling the conversion constructor JacobianFactor(const HessianFactor&).
-   * Variables are eliminated in the natural order of the variable indices of in
-   * the factors.
-   * @param factors Factors to combine and eliminate
-   * @param nrFrontals Number of frontal variables to eliminate.
-   * @return The conditional and remaining factor
-
-   * \addtogroup LinearSolving
-   */
-  GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateQR(const FactorGraph<
-      GaussianFactor>& factors, size_t nrFrontals = 1);
 
   /**
    * Densely partially eliminate with Cholesky factorization.  JacobianFactors
@@ -273,8 +235,8 @@ namespace gtsam {
 
    * \addtogroup LinearSolving
    */
-  GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph<
+  //GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminatePreferCholesky(const FactorGraph<
-      GaussianFactor>& factors, size_t nrFrontals = 1);
+  //    GaussianFactor>& factors, size_t nrFrontals = 1);
 
   /**
    * Densely partially eliminate with Cholesky factorization.  JacobianFactors
@@ -294,22 +256,22 @@ namespace gtsam {
 
    * \addtogroup LinearSolving
    */
-  GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
+  //GTSAM_EXPORT GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
-      GaussianFactor>& factors, size_t nrFrontals = 1);
+  //    GaussianFactor>& factors, size_t nrFrontals = 1);
 
   /****** Linear Algebra Opeations ******/
 
   /** return A*x */
-  GTSAM_EXPORT Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x);
+  GTSAM_EXPORT Errors operator*(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x);
 
   /** In-place version e <- A*x that overwrites e. */
-  GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e);
+  GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, Errors& e);
 
   /** In-place version e <- A*x that takes an iterator. */
-  GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
+  GTSAM_EXPORT void multiplyInPlace(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x, const Errors::iterator& e);
 
   /** x += alpha*A'*e */
-  GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
+  GTSAM_EXPORT void transposeMultiplyAdd(const GaussianFactorGraphUnordered& fg, double alpha, const Errors& e, VectorValuesUnordered& x);
 
   /**
    * Compute the gradient of the energy function,
@@ -318,9 +280,10 @@ namespace gtsam {
    * The gradient is \f$ A^T(Ax-b) \f$.
    * @param fg The Jacobian factor graph $(A,b)$
    * @param x0 The center about which to compute the gradient
-   * @return The gradient as a VectorValues
+   * @return The gradient as a VectorValuesUnordered
+
    */
-  GTSAM_EXPORT VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0);
+  GTSAM_EXPORT VectorValuesUnordered gradient(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x0);
 
   /**
    * Compute the gradient of the energy function,
@@ -328,23 +291,19 @@ namespace gtsam {
    * centered around zero.
    * The gradient is \f$ A^T(Ax-b) \f$.
    * @param fg The Jacobian factor graph $(A,b)$
-   * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
+   * @param [output] g A VectorValuesUnordered to store the gradient, which must be preallocated, see allocateVectorValues
-   * @return The gradient as a VectorValues
+   * @return The gradient as a VectorValuesUnordered
+
    */
-  GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g);
+  GTSAM_EXPORT void gradientAtZero(const GaussianFactorGraphUnordered& fg, VectorValuesUnordered& g);
 
   /* matrix-vector operations */
-  GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
+  GTSAM_EXPORT void residual(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);
-  GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
+  GTSAM_EXPORT void multiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &x, VectorValuesUnordered &r);
-  GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x);
+  GTSAM_EXPORT void transposeMultiply(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered &r, VectorValuesUnordered &x);
-
-  /** shared pointer version
-   * \todo Make this a member function - affects SubgraphPreconditioner */
-  GTSAM_EXPORT boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x);
 
   /** return A*x-b
    * \todo Make this a member function - affects SubgraphPreconditioner */
-  inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) {
+  GTSAM_EXPORT Errors gaussianErrors(const GaussianFactorGraphUnordered& fg, const VectorValuesUnordered& x);
-    return *gaussianErrors_(fg, x); }
 
 } // namespace gtsam

gtsam/linear/GaussianFactorUnordered.h

@@ -26,7 +26,7 @@
 namespace gtsam {
 
   // Forward declarations
-  class VectorValues;
+  class VectorValuesUnordered;
 
   /**
    * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a

gtsam/linear/JacobianFactorUnordered-inl.h

@@ -22,6 +22,7 @@
 #include <boost/range/adaptor/transformed.hpp>
 #include <boost/range/join.hpp>
 #include <boost/assign/list_of.hpp>
+#include <boost/foreach.hpp>
 
 namespace gtsam {
 
@@ -33,13 +34,13 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<typename KEYS, class MATRIX>
+  template<typename KEYS>
   JacobianFactorUnordered::JacobianFactorUnordered(
-    const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) :
+    const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
   Base(keys)
   {
     // Check noise model dimension
-    if(noiseModel && model->dim() != augmentedMatrix.rows())
+    if(model && model->dim() != augmentedMatrix.rows())
       throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
 
     // Check number of variables
@@ -62,13 +63,20 @@ namespace gtsam {
     model_ = model;
   }
 
+  /* ************************************************************************* */
+  namespace {
+    const Matrix& _getPairSecond(const std::pair<Key,Matrix>& p) {
+      return p.second;
+    }
+  }
+
   /* ************************************************************************* */
   template<typename TERMS>
   void JacobianFactorUnordered::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
   {
     // Check noise model dimension
-    if(noiseModel && model->dim() != b.size())
+    if(noiseModel && noiseModel->dim() != b.size())
-      throw InvalidNoiseModel(b.size(), model->dim());
+      throw InvalidNoiseModel(b.size(), noiseModel->dim());
 
     // Resize base class key vector
     Base::keys_.resize(terms.size());
@@ -79,10 +87,14 @@ namespace gtsam {
     using boost::adaptors::map_values;
     using boost::adaptors::transformed;
     using boost::join;
-    Ab_ = VerticalBlockMatrix(join(terms | map_values | transformed(Matrix::cols), cref_list_of<1>(1)), b.size());
+    Ab_ = VerticalBlockMatrix(join(
+      terms
+      | transformed(&_getPairSecond)
+      | transformed(boost::mem_fn(&Matrix::cols)),
+      cref_list_of<1>((DenseIndex)1)), b.size());
 
     // Check and add terms
-    typedef pair<Key, Matrix> Term;
+    typedef std::pair<Key, Matrix> Term;
     DenseIndex i = 0; // For block index
     BOOST_FOREACH(const Term& term, terms) {
       // Check block rows

gtsam/linear/JacobianFactorUnordered.cpp

@@ -24,6 +24,7 @@
 #include <gtsam/linear/GaussianFactorGraphUnordered.h>
 #include <gtsam/linear/VectorValuesUnordered.h>
 #include <gtsam/inference/VariableSlots.h>
+#include <gtsam/inference/OrderingUnordered.h>
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>
 #include <gtsam/base/Matrix.h>
@@ -44,6 +45,7 @@
 #pragma GCC diagnostic pop
 #endif
 #include <boost/range/algorithm/copy.hpp>
+#include <boost/range/adaptor/indirected.hpp>
 
 #include <cmath>
 #include <sstream>
@@ -63,7 +65,6 @@ namespace gtsam {
     //  *this = JacobianFactorUnordered(*rhs);
     else
       throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
-    assertInvariants();
   }
 
   /* ************************************************************************* */
@@ -81,7 +82,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
+  JacobianFactorUnordered::JacobianFactorUnordered(const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
     const Vector& b, const SharedDiagonal& model)
   {
     fillTerms(cref_list_of<2>
@@ -90,7 +91,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  JacobianFactorUnordered::JacobianFactorUnordered(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
+  JacobianFactorUnordered::JacobianFactorUnordered(const Key i1, const Matrix& A1, Key i2, const Matrix& A2,
       Key i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model)
   {
     fillTerms(cref_list_of<3>
@@ -126,28 +127,28 @@ namespace gtsam {
   /* ************************************************************************* */
   // Helper functions for combine constructor
   namespace {
-    boost::tuple<vector<size_t>, size_t, size_t> _countDims(
+    boost::tuple<vector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
-      const std::vector<JacobianFactorUnordered::shared_ptr>& factors, const VariableSlots& variableSlots)
+      const std::vector<JacobianFactorUnordered::shared_ptr>& factors, const vector<VariableSlots::const_iterator>& variableSlots)
     {
       gttic(countDims);
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
-      vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
+      vector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
 #else
-      vector<size_t> varDims(variableSlots.size());
+      vector<DenseIndex> varDims(variableSlots.size());
 #endif
-      size_t m = 0;
+      DenseIndex m = 0;
-      size_t n = 0;
+      DenseIndex n = 0;
       {
         size_t jointVarpos = 0;
-        BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
+        BOOST_FOREACH(VariableSlots::const_iterator slots, variableSlots)
-
+        {
-          assert(slots.second.size() == factors.size());
+          assert(slots->second.size() == factors.size());
 
           size_t sourceFactorI = 0;
-          BOOST_FOREACH(const size_t sourceVarpos, slots.second) {
+          BOOST_FOREACH(const size_t sourceVarpos, slots->second) {
             if(sourceVarpos < numeric_limits<size_t>::max()) {
               const JacobianFactorUnordered& sourceFactor = *factors[sourceFactorI];
-              size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
+              DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
               if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
                 varDims[jointVarpos] = vardim;
@@ -192,7 +193,9 @@ namespace gtsam {
 
   /* ************************************************************************* */
   JacobianFactorUnordered::JacobianFactorUnordered(
-    const GaussianFactorGraphUnordered& factors, boost::optional<const VariableSlots&> variableSlots)
+    const GaussianFactorGraphUnordered& graph,
+    boost::optional<const OrderingUnordered&> ordering,
+    boost::optional<const VariableSlots&> variableSlots)
   {
     gttic(JacobianFactorUnordered_combine_constructor);
 
@@ -200,36 +203,70 @@ namespace gtsam {
     gttic(Compute_VariableSlots);
     boost::optional<VariableSlots> computedVariableSlots;
     if(!variableSlots) {
-      computedVariableSlots = VariableSlots(factors);
+      computedVariableSlots = VariableSlots(graph);
       variableSlots = computedVariableSlots; // Binds reference, does not copy VariableSlots
     }
     gttoc(Compute_VariableSlots);
 
     // Cast or convert to Jacobians
-    std::vector<JacobianFactorUnordered::shared_ptr> jacobians = _convertOrCastToJacobians(factors);
+    std::vector<JacobianFactorUnordered::shared_ptr> jacobians = _convertOrCastToJacobians(graph);
+
+    // Order variable slots
+    vector<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
+      BOOST_FOREACH(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);
+    }
 
     // Count dimensions
-    vector<size_t> varDims;
+    vector<DenseIndex> varDims;
-    size_t m, n;
+    DenseIndex m, n;
-    boost::tie(varDims, m, n) = _countDims(jacobians, *variableSlots);
+    boost::tie(varDims, m, n) = _countDims(jacobians, orderedSlots);
 
+    // Allocate matrix and copy keys in order
     gttic(allocate);
-    Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1>(1)), m); // Allocate augmented matrix
+    Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1>((DenseIndex)1)), m); // Allocate augmented matrix
-    Base::keys_.resize(variableSlots->size());
+    Base::keys_.resize(orderedSlots.size());
-    boost::range::copy(*variableSlots | boost::adaptors::map_keys, Base::keys_.begin()); // Get variable keys from VariableSlots
+    boost::range::copy(orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin()); // Get variable keys
     gttoc(allocate);
 
+    // Loop over slots in combined factor and copy blocks from source factors
     gttic(copy_blocks);
-    // Loop over slots in combined factor
     size_t combinedSlot = 0;
-    BOOST_FOREACH(const VariableSlots::value_type& varslot, variableSlots) {
+    BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) {
       JacobianFactorUnordered::ABlock destSlot(this->getA(this->begin()+combinedSlot));
       // Loop over source jacobians
-      size_t nextRow = 0;
+      DenseIndex nextRow = 0;
       for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
         // Slot in source factor
-        const size_t sourceSlot = varslot.second[factorI];
+        const size_t sourceSlot = varslot->second[factorI];
-        const size_t sourceRows = jacobians[factorI]->rows();
+        const DenseIndex sourceRows = jacobians[factorI]->rows();
         JacobianFactorUnordered::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
         // Copy if exists in source factor, otherwise set zero
         if(sourceSlot != numeric_limits<size_t>::max())
@@ -242,13 +279,14 @@ namespace gtsam {
     }
     gttoc(copy_blocks);
 
+    // Copy the RHS vectors and sigmas
     gttic(copy_vectors);
     bool anyConstrained = false;
     boost::optional<Vector> sigmas;
     // Loop over source jacobians
-    size_t nextRow = 0;
+    DenseIndex nextRow = 0;
     for(size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
-      const size_t sourceRows = jacobians[factorI]->rows();
+      const DenseIndex sourceRows = jacobians[factorI]->rows();
       this->getb().segment(nextRow, sourceRows) = jacobians[factorI]->getb();
       if(jacobians[factorI]->get_model()) {
         // If the factor has a noise model and we haven't yet allocated sigmas, allocate it.
@@ -421,8 +459,53 @@ namespace gtsam {
   //}
 
   /* ************************************************************************* */
-  GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminateFirst() {
+  std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
-    return this->eliminate(1);
+    JacobianFactorUnordered::eliminate(const OrderingUnordered& keys)
+  {
+    GaussianFactorGraphUnordered graph;
+    graph.add(*this);
+    return EliminateQRUnordered(graph, keys);
+  }
+
+  /* ************************************************************************* */
+  void JacobianFactorUnordered::setModel(bool anyConstrained, const Vector& sigmas) {
+    if((size_t) sigmas.size() != this->rows())
+      throw InvalidNoiseModel(this->rows(), sigmas.size());
+    if (anyConstrained)
+      model_ = noiseModel::Constrained::MixedSigmas(sigmas);
+    else
+      model_ = noiseModel::Diagonal::Sigmas(sigmas);
+  }
+
+  /* ************************************************************************* */
+  std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
+    EliminateQRUnordered(const GaussianFactorGraphUnordered& factors, const OrderingUnordered& keys)
+  {
+    // Combine and sort variable blocks in elimination order
+    JacobianFactorUnordered::shared_ptr jointFactor;
+    try {
+      jointFactor = boost::make_shared<JacobianFactorUnordered>(factors, keys);
+    } catch(std::invalid_argument& e) {
+      (void) e; // Avoid unused variable warning
+      throw InvalidDenseElimination(
+        "EliminateQRUnordered was called with a request to eliminate variables that are not\n"
+        "involved in the provided factors.");
+    }
+
+    // Do dense elimination
+    SharedDiagonal noiseModel;
+    if(jointFactor->model_)
+      jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix());
+    else
+      inplace_QR(jointFactor->Ab_.matrix());
+
+    // Zero below the diagonal
+    jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
+
+    // Split elimination result into conditional and remaining factor
+    GaussianConditionalUnordered::shared_ptr conditional = jointFactor->splitConditional(keys.size());
+
+    return make_pair(conditional, jointFactor);
   }
 
   /* ************************************************************************* */
@@ -441,9 +524,12 @@ namespace gtsam {
     gttic(cond_Rd);
     const DenseIndex originalRowEnd = Ab_.rowEnd();
     Ab_.rowEnd() = Ab_.rowStart() + frontalDim;
-    const Eigen::VectorBlock<const Vector> sigmas = model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart());
+    SharedDiagonal conditionalNoiseModel;
-    GaussianConditionalUnordered::shared_ptr conditional(boost::make_shared<GaussianConditionalUnordered>(
+    if(model_)
-      Base::keys_, nrFrontals, Ab_, sigmas));
+      conditionalNoiseModel =
+      noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rowEnd()-Ab_.rowStart()));
+    GaussianConditionalUnordered::shared_ptr conditional = boost::make_shared<GaussianConditionalUnordered>(
+      Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
     Ab_.rowStart() += frontalDim;
     Ab_.firstBlock() += nrFrontals;
     Ab_.rowEnd() = originalRowEnd;
@@ -464,63 +550,4 @@ namespace gtsam {
     return conditional;
   }
 
-  /* ************************************************************************* */
-  GaussianConditionalUnordered::shared_ptr JacobianFactorUnordered::eliminate(size_t nrFrontals) {
-
-    if(Ab_.rowStart() != 0 || Ab_.rowEnd() != (size_t) Ab_.matrix().rows() && Ab_.firstBlock() == 0);
-    if(nrFrontals > size())
-      throw std::invalid_argument("Requesting to eliminate more variables than exist using JacobianFactor::splitConditional");
-    assertInvariants();
-
-    const bool debug = ISDEBUG("JacobianFactor::eliminate");
-
-    if(debug) cout << "Eliminating " << nrFrontals << " frontal variables" << endl;
-    if(debug) this->print("Eliminating JacobianFactor: ");
-    if(debug) gtsam::print(matrix_, "Augmented Ab: ");
-
-    size_t frontalDim = Ab_.range(0,nrFrontals).cols();
-
-    if(debug) cout << "frontalDim = " << frontalDim << endl;
-
-    // Use in-place QR dense Ab appropriate to NoiseModel
-    gttic(QR);
-    SharedDiagonal noiseModel = model_->QR(matrix_);
-    gttoc(QR);
-
-    // Zero the lower-left triangle.  todo: not all of these entries actually
-    // need to be zeroed if we are careful to start copying rows after the last
-    // structural zero.
-    if(matrix_.rows() > 0)
-      for(size_t j=0; j<(size_t) matrix_.cols(); ++j)
-        for(size_t i=j+1; i<noiseModel->dim(); ++i)
-          matrix_(i,j) = 0.0;
-
-    if(debug) gtsam::print(matrix_, "QR result: ");
-    if(debug) noiseModel->print("QR result noise model: ");
-
-    // Start of next part
-    model_ = noiseModel;
-    return splitConditional(nrFrontals);
-  }
-
-  /* ************************************************************************* */
-  //void JacobianFactorUnordered::allocate(const VariableSlots& variableSlots, vector<
-  //    size_t>& varDims, size_t m) {
-  //  keys_.resize(variableSlots.size());
-  //  std::transform(variableSlots.begin(), variableSlots.end(), begin(),
-  //      boost::bind(&VariableSlots::const_iterator::value_type::first, _1));
-  //  varDims.push_back(1);
-  //  Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m));
-  //}
-
-  /* ************************************************************************* */
-  void JacobianFactorUnordered::setModel(bool anyConstrained, const Vector& sigmas) {
-    if((size_t) sigmas.size() != this->rows())
-      throw InvalidNoiseModel(this->rows(), sigmas.size());
-    if (anyConstrained)
-      model_ = noiseModel::Constrained::MixedSigmas(sigmas);
-    else
-      model_ = noiseModel::Diagonal::Sigmas(sigmas);
-  }
-
 }

gtsam/linear/JacobianFactorUnordered.h

@@ -33,6 +33,7 @@ namespace gtsam {
   class GaussianFactorGraphUnordered;
   class GaussianConditionalUnordered;
   class VectorValuesUnordered;
+  class OrderingUnordered;
 
   /**
    * A Gaussian factor in the squared-error form.
@@ -136,6 +137,7 @@ namespace gtsam {
      * computation performed. */
     explicit JacobianFactorUnordered(
       const GaussianFactorGraphUnordered& graph,
+      boost::optional<const OrderingUnordered&> ordering = boost::none,
       boost::optional<const VariableSlots&> variableSlots = boost::none);
 
     /** Virtual destructor */
@@ -260,16 +262,35 @@ namespace gtsam {
     std::vector<boost::tuple<size_t, size_t, double> >
     sparse(const std::vector<size_t>& columnIndices) const;
 
-    /**
+    /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
-     * Return a whitened version of the factor, i.e. with unit diagonal noise
-     * model. */
     JacobianFactorUnordered whiten() const;
 
-    /** Eliminate the first variable, modifying the factor in place to contain the remaining marginal. */
+    /** Eliminate the requested variables. */
-    boost::shared_ptr<GaussianConditionalUnordered> eliminateFirst();
+    std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
+      eliminate(const OrderingUnordered& keys);
 
-    /** Eliminate the requested number of frontal variables, modifying the factor in place to contain the remaining marginal. */
+    /** set noiseModel correctly */
-    boost::shared_ptr<GaussianConditionalUnordered> eliminate(size_t nrFrontals = 1);
+    void setModel(bool anyConstrained, const Vector& sigmas);
+    
+  /**
+   * Densely partially eliminate with QR factorization, this is usually provided as an argument to
+   * one of the factor graph elimination functions (see EliminateableFactorGraph).  HessianFactors
+   * are first factored with Cholesky decomposition to produce JacobianFactors, by calling the
+   * conversion constructor JacobianFactor(const HessianFactor&). Variables are eliminated in the
+   * order specified in \c keys.
+   * @param factors Factors to combine and eliminate
+   * @param keys The variables to eliminate in the order as specified here in \c keys
+   * @return The conditional and remaining factor
+   *
+   * \addtogroup LinearSolving */
+    friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditionalUnordered>, boost::shared_ptr<JacobianFactorUnordered> >
+      EliminateQRUnordered(const GaussianFactorGraphUnordered& factors, const OrderingUnordered& keys);
+
+  private:
+
+    /// Internal function to fill blocks and set dimensions
+    template<typename TERMS>
+    void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
     
     /**
      * splits a pre-factorized factor into a conditional, and changes the current
@@ -278,25 +299,6 @@ namespace gtsam {
      */
     boost::shared_ptr<GaussianConditionalUnordered> splitConditional(size_t nrFrontals = 1);
 
-    // following methods all used in CombineJacobians:
-    // Many imperative, perhaps all need to be combined in constructor
-
-    /** allocate space */
-    //void allocate(const VariableSlots& variableSlots,
-    //    std::vector<size_t>& varDims, size_t m);
-
-    /** set noiseModel correctly */
-    void setModel(bool anyConstrained, const Vector& sigmas);
-
-    /** Assert invariants after elimination */
-    void assertInvariants() const;
-
-  private:
-
-    /// Internal function to fill blocks and set dimensions
-    template<typename TERMS>
-    void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
-
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>

gtsam/linear/VectorValuesUnordered.cpp

@@ -131,33 +131,6 @@ bool VectorValuesUnordered::hasSameStructure(const VectorValuesUnordered& other)
   return true;
 }
 
-/* ************************************************************************* */
-void VectorValuesUnordered::permuteInPlace(const Permutation& permutation) {
-  // Allocate new values
-  Values newValues(this->size());
-
-  // Swap values from this VectorValues to the permuted VectorValues
-  for(size_t i = 0; i < this->size(); ++i)
-    newValues[i].swap(this->at(permutation[i]));
-
-  // Swap the values into this VectorValues
-  values_.swap(newValues);
-}
-
-/* ************************************************************************* */
-void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
-  if(selector.size() != permutation.size())
-    throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
-  // Create new index the size of the permuted entries
-  Values reorderedEntries(selector.size());
-  // Permute the affected entries into the new index
-  for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
-    reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]);
-  // Put the affected entries back in the new order
-  for(size_t slot = 0; slot < selector.size(); ++slot)
-    values_[selector[slot]].swap(reorderedEntries[slot]);
-}
-
 /* ************************************************************************* */
 void VectorValuesUnordered::swap(VectorValuesUnordered& other) {
   this->values_.swap(other.values_);

gtsam/linear/VectorValuesUnordered.h

@@ -181,7 +181,7 @@ namespace gtsam {
 
     /** Named constructor to create a VectorValues that matches the structure of
      * the specified VectorValues, but do not initialize the new values. */
-    static VectorValuesUnordered SameStructure(const VectorValues& other);
+    static VectorValuesUnordered SameStructure(const VectorValuesUnordered& other);
 
     /** Named constructor to create a VectorValues from a container of variable
      * dimensions that is filled with zeros.
@@ -318,7 +318,7 @@ namespace gtsam {
      * scale a vector by a scalar
      */
     friend VectorValuesUnordered operator*(const double a, const VectorValuesUnordered &v) {
-      VectorValues result = VectorValues::SameStructure(v);
+      VectorValuesUnordered result = VectorValuesUnordered::SameStructure(v);
       for(Index j = 0; j < v.size(); ++j)
         result.values_[j] = a * v.values_[j];
       return result;
@@ -415,7 +415,7 @@ namespace gtsam {
     // in the first and last iterators, and concatenates them in that order into the
     // output.
     template<typename ITERATOR>
-    const VectorValuesUnordered extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
+    const Vector extractVectorValuesSlices(const VectorValuesUnordered& values, ITERATOR first, ITERATOR last, bool allowNonexistant = false) {
       // Find total dimensionality
       size_t dim = 0;
       for(ITERATOR j = first; j != last; ++j)

gtsam/linear/linearExceptions.h

@@ -141,7 +141,7 @@ on gtsam::IndeterminantLinearSystemException for more information.\n";
     const DenseIndex blockRows; ///< The dimensionality of the noise model
 
     InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) :
-      factorRows(factorRows), blockRows(noiseModelDims) {}
+      factorRows(factorRows), blockRows(blockRows) {}
     virtual ~InvalidMatrixBlock() throw() {}
 
     virtual const char* what() const throw();
@@ -150,4 +150,10 @@ on gtsam::IndeterminantLinearSystemException for more information.\n";
     mutable std::string description_;
   };
 
+  /* ************************************************************************* */
+  class InvalidDenseElimination : public std::invalid_argument {
+  public:
+    InvalidDenseElimination(const char *message) : std::invalid_argument(message) {}
+  };
+
  }

gtsam/symbolic/SymbolicFactorGraphUnordered.h

@@ -35,9 +35,10 @@ namespace gtsam {
   class SymbolicJunctionTreeUnordered;
 
   /* ************************************************************************* */
-  template<> class EliminationTraits<SymbolicFactorGraphUnordered>
+  template<> struct EliminationTraits<SymbolicFactorGraphUnordered>
   {
     typedef SymbolicFactorUnordered FactorType;                   ///< Type of factors in factor graph
+    typedef SymbolicFactorGraphUnordered FactorGraphType;         ///< Type of the factor graph (e.g. GaussianFactorGraph)
     typedef SymbolicConditionalUnordered ConditionalType;         ///< Type of conditionals from elimination
     typedef SymbolicBayesNetUnordered BayesNetType;               ///< Type of Bayes net from sequential elimination
     typedef SymbolicEliminationTreeUnordered EliminationTreeType; ///< Type of elimination tree
@@ -45,8 +46,8 @@ namespace gtsam {
     typedef SymbolicJunctionTreeUnordered JunctionTreeType;       ///< Type of Junction tree
     /// The default dense elimination function
     static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
-      DefaultEliminate(const std::vector<boost::shared_ptr<FactorType> >& factors,
+      DefaultEliminate(const FactorGraphType& factors, const OrderingUnordered& keys) {
-      const std::vector<Key>& keys) { return EliminateSymbolicUnordered(factors, keys); }
+        return EliminateSymbolicUnordered(factors, keys); }
   };
 
   /* ************************************************************************* */

gtsam/symbolic/SymbolicFactorUnordered.cpp

@@ -18,8 +18,10 @@
 #include <boost/foreach.hpp>
 #include <boost/make_shared.hpp>
 
+#include <gtsam/inference/OrderingUnordered.h>
 #include <gtsam/symbolic/SymbolicFactorUnordered.h>
 #include <gtsam/symbolic/SymbolicConditionalUnordered.h>
+#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
 
 using namespace std;
 
@@ -27,7 +29,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
-    EliminateSymbolicUnordered(const vector<SymbolicFactorUnordered::shared_ptr>& factors, const vector<Key>& keys)
+    EliminateSymbolicUnordered(const SymbolicFactorGraphUnordered& factors, const OrderingUnordered& keys)
   {
     // Gather all keys
     FastSet<Key> allKeys;

gtsam/symbolic/SymbolicFactorUnordered.h

@@ -26,7 +26,7 @@
 
 namespace gtsam {
 
-  // Forward declaration of SymbolicConditional
+  // Forward declarations
   class SymbolicConditionalUnordered;
 
   /**
@@ -114,11 +114,15 @@ namespace gtsam {
     }
   }; // IndexFactor
 
+  // Forward declarations
+  class SymbolicFactorGraphUnordered;
+  class OrderingUnordered;
+
   /** Dense elimination function for symbolic factors.  This is usually provided as an argument to
    *  one of the factor graph elimination functions (see EliminateableFactorGraph).  The factor
    *  graph elimination functions do sparse variable elimination, and use this function to eliminate
    *  single variables or variable cliques. */
   GTSAM_EXPORT std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
-    EliminateSymbolicUnordered(const std::vector<SymbolicFactorUnordered::shared_ptr>& factors, const std::vector<Key>& keys);
+    EliminateSymbolicUnordered(const SymbolicFactorGraphUnordered& factors, const OrderingUnordered& keys);
 
 }