From 610cd5f1d36d4ce16bc6dbef60f534177f81a2b7 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Tue, 29 Dec 2015 08:46:16 -0800
Subject: [PATCH] Output matrix in right order

---
 gtsam/linear/GaussianBayesNet.cpp | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index b3b8b9a41..6db13adb6 100644
--- a/gtsam/linear/GaussianBayesNet.cpp
+++ b/gtsam/linear/GaussianBayesNet.cpp
@@ -141,7 +141,20 @@ namespace gtsam {
   /* ************************************************************************* */  
   pair<Matrix,Vector> GaussianBayesNet::matrix() const
   {
-    return GaussianFactorGraph(*this).jacobian();
+    GaussianFactorGraph factorGraph(*this);
+    KeySet keys = factorGraph.keys();
+    // add frontal keys in order
+    Ordering ordering;
+    BOOST_FOREACH (const sharedConditional& cg, *this)
+      BOOST_FOREACH (Key key, cg->frontals()) {
+        ordering.push_back(key);
+        keys.erase(key);
+      }
+    // add remaining keys in case Bayes net is incomplete
+    BOOST_FOREACH (Key key, keys)
+      ordering.push_back(key);
+    // return matrix and RHS
+    return factorGraph.jacobian(ordering);
   }
 
   ///* ************************************************************************* */