Added optional ordering argument when converting to Matrix/Vector

gtsam/linear/GaussianBayesNet.cpp

@@ -138,23 +138,33 @@ namespace gtsam {
   //}
 
   /* ************************************************************************* */
-  pair<Matrix, Vector> GaussianBayesNet::matrix() const {
+  Ordering GaussianBayesNet::ordering() const {
     GaussianFactorGraph factorGraph(*this);
-    KeySet keys = factorGraph.keys();
+    auto keys = factorGraph.keys();
     // add frontal keys in order
     Ordering ordering;
-    for (const sharedConditional& cg: *this)
+    for (const sharedConditional& cg : *this)
       if (cg) {
-        for (Key key: cg->frontals()) {
+        for (Key key : cg->frontals()) {
           ordering.push_back(key);
           keys.erase(key);
         }
       }
     // add remaining keys in case Bayes net is incomplete
-    for (Key key: keys)
-      ordering.push_back(key);
-    // return matrix and RHS
-    return factorGraph.jacobian(ordering);
+    for (Key key : keys) ordering.push_back(key);
+    return ordering;
+  }
+
+  /* ************************************************************************* */
+  pair<Matrix, Vector> GaussianBayesNet::matrix(boost::optional<const Ordering&> ordering) const {
+    if (ordering) {
+      // Convert to a GaussianFactorGraph and use its machinery
+      GaussianFactorGraph factorGraph(*this);
+      return factorGraph.jacobian(ordering);
+    } else {
+      // recursively call with default ordering
+      return matrix(this->ordering());
+    }
   }
 
   ///* ************************************************************************* */

gtsam/linear/GaussianBayesNet.h

@@ -74,6 +74,14 @@ namespace gtsam {
     /// Version of optimize for incomplete BayesNet, needs solution for missing variables
     VectorValues optimize(const VectorValues& solutionForMissing) const;
 
+    /**
+     * Return ordering corresponding to a topological sort.
+     * There are many topological sorts of a Bayes net. This one
+     * corresponds to the one that makes 'matrix' below upper-triangular.
+     * In case Bayes net is incomplete any non-frontal are added to the end.
+     */
+    Ordering ordering() const;
+
     ///@}
 
     ///@name Linear Algebra
@@ -81,8 +89,10 @@ namespace gtsam {
     
     /**
      * Return (dense) upper-triangular matrix representation
+     * Will return upper-triangular matrix only when using 'ordering' above.
+     * In case Bayes net is incomplete zero columns are added to the end.
      */
-    std::pair<Matrix, Vector> matrix() const;
+    std::pair<Matrix, Vector> matrix(boost::optional<const Ordering&> ordering = boost::none) const;
 
     /**
      * Optimize along the gradient direction, with a closed-form computation to perform the line

gtsam/linear/VectorValues.cpp

@@ -142,19 +142,25 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Vector VectorValues::vector() const
-  {
+  Vector VectorValues::vector(boost::optional<const Ordering&> ordering) const {
     // Count dimensions
     DenseIndex totalDim = 0;
-    for(const Vector& v: *this | map_values)
-      totalDim += v.size();
+    for (const Vector& v : *this | map_values) totalDim += v.size();
 
     // Copy vectors
     Vector result(totalDim);
     DenseIndex pos = 0;
-    for(const Vector& v: *this | map_values) {
-      result.segment(pos, v.size()) = v;
-      pos += v.size();
+    if (ordering) {
+      for (const auto& key : *ordering) {
+        const auto& v = (*this)[key];
+        result.segment(pos, v.size()) = v;
+        pos += v.size();
+      }
+    } else {
+      for (const Vector& v : *this | map_values) {
+        result.segment(pos, v.size()) = v;
+        pos += v.size();
+      }
     }
 
     return result;

gtsam/linear/VectorValues.h

@@ -244,7 +244,7 @@ namespace gtsam {
     /// @{
 
     /** Retrieve the entire solution as a single vector */
-    Vector vector() const;
+    Vector vector(boost::optional<const Ordering&> ordering = boost::none) const;
 
     /** Access a vector that is a subset of relevant keys. */
     template <typename CONTAINER>