From df1eede30c81092e4bc0aa48b8b66eef3bb1db27 Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Sat, 13 Sep 2014 01:37:17 -0400
Subject: [PATCH] Produce a graph of dual-scaled Hessians of each factor:
 -lambda*H, used for solving nonlinear equality constraints with SQP.

---
 gtsam/nonlinear/NonlinearFactorGraph.cpp | 16 ++++++++++++++++
 gtsam/nonlinear/NonlinearFactorGraph.h   |  7 +++++++
 2 files changed, 23 insertions(+)

diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp
index 63a1a2218..55251f9dc 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.cpp
+++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp
@@ -365,6 +365,22 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li
   return linearFG;
 }
 
+/* ************************************************************************* */
+boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph::multipliedHessians(
+        const Values& linearizationPoint, const VectorValues& duals) const {
+  GaussianFactorGraph::shared_ptr hessianFG = boost::make_shared<GaussianFactorGraph>();
+  hessianFG->reserve(this->size());
+
+  // create multiplied Hessians for all factors
+  BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
+    if(factor) {
+      (*hessianFG) += factor->multipliedHessian(linearizationPoint, duals);
+    } else
+      (*hessianFG) += GaussianFactor::shared_ptr();
+  }
+  return hessianFG;
+}
+
 /* ************************************************************************* */
 NonlinearFactorGraph NonlinearFactorGraph::clone() const {
   NonlinearFactorGraph result;
diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h
index 35e532262..3ac0aeb07 100644
--- a/gtsam/nonlinear/NonlinearFactorGraph.h
+++ b/gtsam/nonlinear/NonlinearFactorGraph.h
@@ -134,6 +134,13 @@ namespace gtsam {
      */
     boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
 
+    /**
+     * Produce a graph of dual-scaled Hessians of each factor: lambda*H,
+     * used for solving nonlinear equality constraints using SQP.
+     */
+    boost::shared_ptr<GaussianFactorGraph> multipliedHessians(
+        const Values& linearizationPoint, const VectorValues& duals) const;
+
     /**
      * Clone() performs a deep-copy of the graph, including all of the factors
      */