12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Produce a graph of dual-scaled Hessians of each factor: -lambda*H, used for solving nonlinear equality constraints with SQP.

release/4.3a0
thduynguyen 2014-09-13 01:37:17 -04:00
parent 20fb8ab77d
commit df1eede30c
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -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;

7
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -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
*/