SubgraphSolver.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/linear/SubgraphSolver.h>
 
#include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
 
using namespace std;
 
namespace gtsam {
 
/**************************************************************************************************/
// Just taking system [A|b]
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab,
    const Parameters &parameters, const Ordering& ordering) :
    parameters_(parameters) {
  const auto [Ab1, Ab2] = splitGraph(Ab);
  if (parameters_.verbosity())
    cout << "Split A into (A1) " << Ab1.size() << " and (A2) " << Ab2.size()
         << " factors" << endl;
 
  auto Rc1 = *Ab1.eliminateSequential(ordering, EliminateQR);
  auto xbar = Rc1.optimize();
  pc_ = std::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
 
/**************************************************************************************************/
// Taking eliminated tree [R1|c] and constraint graph [A2|b2]
SubgraphSolver::SubgraphSolver(const GaussianBayesNet &Rc1,
                               const GaussianFactorGraph &Ab2,
                               const Parameters &parameters)
    : parameters_(parameters) {
  auto xbar = Rc1.optimize();
  pc_ = std::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
 
/**************************************************************************************************/
// Taking subgraphs [A1|b1] and [A2|b2]
// delegate up
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
                               const GaussianFactorGraph &Ab2,
                               const Parameters &parameters,
                               const Ordering &ordering)
    : SubgraphSolver(*Ab1.eliminateSequential(ordering, EliminateQR), Ab2,
                     parameters) {}
 
/**************************************************************************************************/
VectorValues SubgraphSolver::optimize() const {
  VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
      Errors>(*pc_, pc_->zero(), parameters_);
  return pc_->x(ybar);
}
 
VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
    const KeyInfo &keyInfo, const map<Key, Vector> &lambda,
    const VectorValues &initial) {
  return VectorValues();
}
/**************************************************************************************************/
pair<GaussianFactorGraph, GaussianFactorGraph> //
SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) {
 
  /* identify the subgraph structure */
  const SubgraphBuilder builder(parameters_.builderParams);
  auto subgraph = builder(factorGraph);
 
  /* build factor subgraph */
  return splitFactorGraph(factorGraph, subgraph);
}
 
/****************************************************************************/
 
} // \namespace gtsam