From 2b1f7f84464a5d97d303cfaba9ec67c7b2c2f096 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 16 Sep 2011 05:32:04 +0000
Subject: [PATCH] Added GaussianFactorGraph::denseHessian to get factor graph
 hessian

---
 gtsam/linear/GaussianFactorGraph.cpp | 16 ++++++++++++++++
 gtsam/linear/GaussianFactorGraph.h   |  7 ++++++-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 441962af4..bcfa32610 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -290,6 +290,22 @@ namespace gtsam {
     return scatter;
   }
 
+  /* ************************************************************************* */
+  Matrix GaussianFactorGraph::denseHessian() const {
+
+    Scatter scatter = findScatterAndDims(*this);
+
+    vector<size_t> dims; dims.reserve(scatter.size() + 1);
+    BOOST_FOREACH(const Scatter::value_type& index_entry, scatter) {
+      dims.push_back(index_entry.second.dimension);
+    }
+    dims.push_back(1);
+
+    // combine all factors
+    HessianFactor combined(*this, dims, scatter);
+    return combined.info();
+  }
+
 	/* ************************************************************************* */
 	GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
 			GaussianFactor>& factors, size_t nrFrontals) {
diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h
index 921cb7958..a06bf02d5 100644
--- a/gtsam/linear/GaussianFactorGraph.h
+++ b/gtsam/linear/GaussianFactorGraph.h
@@ -160,11 +160,16 @@ namespace gtsam {
     std::vector<boost::tuple<size_t,size_t,double> > sparseJacobian(const std::vector<size_t>& columnIndices) const;
 
     /**
-     * Return a dense m-by-n Jacobian matrix, augmented with b
+     * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b
      * with standard deviations are baked into A and b
      */
     Matrix denseJacobian() const;
 
+    /**
+     * Return a dense \f$ n \times n \f$ Hessian matrix, augmented with \f$ A^T b \f$
+     */
+    Matrix denseHessian() const;
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;