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 NonlinearFactorGraph::multipliedHessians( + const Values& linearizationPoint, const VectorValues& duals) const { + GaussianFactorGraph::shared_ptr hessianFG = boost::make_shared(); + 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 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 multipliedHessians( + const Values& linearizationPoint, const VectorValues& duals) const; + /** * Clone() performs a deep-copy of the graph, including all of the factors */