SubgraphSolver.h

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

#pragma once


#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
#include <iosfwd>

namespace gtsam {

  // Forward declarations
  class GaussianFactorGraph;
  class GaussianBayesNet;
  class SubgraphPreconditioner;

class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters {
public:
  typedef ConjugateGradientParameters Base;
  SubgraphSolverParameters() : Base() {}
  void print() const { Base::print(); }
  virtual void print(std::ostream &os) const { Base::print(os); }
};

class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {

public:
  typedef SubgraphSolverParameters Parameters;

protected:
  Parameters parameters_;
  Ordering ordering_;
  boost::shared_ptr<SubgraphPreconditioner> pc_;  

public:
  /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
  SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters, const Ordering& ordering);
  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A, const Parameters &parameters, const Ordering& ordering);

  /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */
  SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);

  /* The same as above, but the A1 is solved before */
  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);

  virtual ~SubgraphSolver() {}

  VectorValues optimize () ;
  VectorValues optimize (const VectorValues &initial) ;

  /* interface to the nonlinear optimizer that the subclasses have to implement */
  virtual VectorValues optimize (
    const GaussianFactorGraph &gfg,
    const KeyInfo &keyInfo,
    const std::map<Key, Vector> &lambda,
    const VectorValues &initial
  ) ;

protected:

  void initialize(const GaussianFactorGraph &jfg);
  void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2);

  boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> >
  splitGraph(const GaussianFactorGraph &gfg) ;
};

} // namespace gtsam