Added GaussianFactorGraph::denseHessian to get factor graph hessian

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) {

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;