From e1ef2199160dd138fd75405d95ba893cf3d952da Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 20 Sep 2013 15:25:16 +0000
Subject: [PATCH] Added optional ordering for creating dense jacobian and
 hessian matrices from GaussianFactorGraph

---
 gtsam/linear/GaussianFactorGraph.cpp | 16 ++++++++--------
 gtsam/linear/GaussianFactorGraph.h   |  8 ++++----
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 9e16e7e8c..d1505a89d 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -134,24 +134,24 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Matrix GaussianFactorGraph::augmentedJacobian() const {
+  Matrix GaussianFactorGraph::augmentedJacobian(boost::optional<const Ordering&> optionalOrdering) const {
     // combine all factors
-    JacobianFactor combined(*this);
+    JacobianFactor combined(*this, optionalOrdering);
     return combined.augmentedJacobian();
   }
 
   /* ************************************************************************* */
-  std::pair<Matrix,Vector> GaussianFactorGraph::jacobian() const {
-    Matrix augmented = augmentedJacobian();
+  std::pair<Matrix,Vector> GaussianFactorGraph::jacobian(boost::optional<const Ordering&> optionalOrdering) const {
+    Matrix augmented = augmentedJacobian(optionalOrdering);
     return make_pair(
       augmented.leftCols(augmented.cols()-1),
       augmented.col(augmented.cols()-1));
   }
 
   /* ************************************************************************* */
-  Matrix GaussianFactorGraph::augmentedHessian() const {
+  Matrix GaussianFactorGraph::augmentedHessian(boost::optional<const Ordering&> optionalOrdering) const {
     // combine all factors and get upper-triangular part of Hessian
-    HessianFactor combined(*this);
+    HessianFactor combined(*this, Scatter(*this, optionalOrdering));
     Matrix result = combined.info();
     // Fill in lower-triangular part of Hessian
     result.triangularView<Eigen::StrictlyLower>() = result.transpose();
@@ -159,8 +159,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const {
-    Matrix augmented = augmentedHessian();
+  std::pair<Matrix,Vector> GaussianFactorGraph::hessian(boost::optional<const Ordering&> optionalOrdering) const {
+    Matrix augmented = augmentedHessian(optionalOrdering);
     return make_pair(
       augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
       augmented.col(augmented.rows()-1).head(augmented.rows()-1));
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index d5f3fa051..aaca09b12 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -172,7 +172,7 @@ namespace gtsam {
      * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
      * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
      */
-    Matrix augmentedJacobian() const;
+    Matrix augmentedJacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
 
     /**
      * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
@@ -181,7 +181,7 @@ namespace gtsam {
      * GaussianFactorGraph::augmentedJacobian and
      * GaussianFactorGraph::sparseJacobian.
      */
-    std::pair<Matrix,Vector> jacobian() const;
+    std::pair<Matrix,Vector> jacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
 
     /**
      * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
@@ -194,7 +194,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(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
 
     /**
      * Return the dense Hessian \f$ \Lambda \f$ and information vector
@@ -202,7 +202,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(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
 
     /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
      *  the dense elimination function specified in \c function (default EliminatePreferCholesky),