/* * SubgraphPreconditioner.h * Created on: Dec 31, 2009 * @author: Frank Dellaert */ #ifndef SUBGRAPHPRECONDITIONER_H_ #define SUBGRAPHPRECONDITIONER_H_ #include "GaussianFactorGraph.h" #include "GaussianBayesNet.h" namespace gtsam { /** * Subgraph conditioner class, as explained in the RSS 2010 submission. * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 * We solve R1*x=c1, and make the substitution y=R1*x-c1. * To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2. * Then solve for yhat using CG, and solve for xhat = system.x(yhat). */ class SubgraphPreconditioner { private: const GaussianBayesNet& Rc1_; const GaussianFactorGraph& Ab2_; const VectorConfig& xbar_; const Errors b2bar_; /** b2 - A2*xbar */ public: /** * Constructor * @param Rc1: the Bayes Net R1*x=c1 * @param Ab2: the Graph A2*x=b2 * @param xbar: the solution to R1*x=c1 */ SubgraphPreconditioner(const GaussianBayesNet& Rc1, const GaussianFactorGraph& Ab2, const VectorConfig& xbar); /* x = xbar + inv(R1)*y */ VectorConfig x(const VectorConfig& y) const; /* error, given y */ double error(const VectorConfig& y) const; /** gradient = y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar) */ VectorConfig gradient(const VectorConfig& y) const; /** Apply operator A */ Errors operator*(const VectorConfig& y) const; /** Apply operator A' */ VectorConfig operator^(const Errors& e) const; /** print the object */ void print(const std::string& s = "SubgraphPreconditioner") const; }; class Ordering; /** * A linear system solver using subgraph preconditioning conjugate gradient */ template class SubgraphPCG { private: typedef typename Config::Key Key; typedef typename NonlinearGraph::Constraint Constraint; typedef typename NonlinearGraph::Pose Pose; const size_t maxIterations_; const bool verbose_; const double epsilon_, epsilon_abs_; NonlinearGraph T_, C_; public: SubgraphPCG() {} SubgraphPCG(const NonlinearGraph& G, const Config& config); /** * solve for the optimal displacement in the tangent space, and then solve * the resulted linear system */ VectorConfig optimize(GaussianFactorGraph& fg, const Ordering& ordering) const { throw std::runtime_error("SubgraphPCG:: optimize is not supported!"); } /** * linearize the non-linear graph around the current config, */ VectorConfig linearizeAndOptimize(const NonlinearGraph& g, const Config& config, const Ordering& ordering) const; }; } // nsamespace gtsam #endif /* SUBGRAPHPRECONDITIONER_H_ */