diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 3853a72fa..406109758 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -33,54 +33,56 @@ namespace gtsam { /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters, const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(gfg); + parameters_(parameters) { + + GaussianFactorGraph::shared_ptr Ab1,Ab2; + boost::tie(Ab1, Ab2) = splitGraph(gfg); + if (parameters_.verbosity()) + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; + + auto Rc1 = Ab1->eliminateSequential(ordering, EliminateQR); + auto xbar = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } -/**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, +// delegate up +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &factorGraph, const Parameters ¶meters, const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(*jfg); -} + SubgraphSolver(*factorGraph, parameters, ordering) {} /**************************************************************************************************/ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2, + const Parameters ¶meters) + : parameters_(parameters) { + auto xbar = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); +} + +// delegate up +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, + const Parameters ¶meters) + : SubgraphSolver(Rc1, boost::make_shared(Ab2), + parameters_) {} + +// delegate up SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { + const GaussianFactorGraph &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), + boost::make_shared(Ab2), + parameters_) {} - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, - EliminateQR); - initialize(Rc1, boost::make_shared(Ab2)); -} - -/**************************************************************************************************/ +// delegate up SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, - EliminateQR); - initialize(Rc1, Ab2); -} - -/**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(Rc1, boost::make_shared(Ab2)); -} - -/**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(Rc1, Ab2); -} + const GaussianFactorGraph::shared_ptr &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1->eliminateSequential(ordering, EliminateQR), Ab2, + parameters) {} /**************************************************************************************************/ VectorValues SubgraphSolver::optimize() { @@ -89,38 +91,16 @@ VectorValues SubgraphSolver::optimize() { return pc_->x(ybar); } -/**************************************************************************************************/ VectorValues SubgraphSolver::optimize(const VectorValues &initial) { // the initial is ignored in this case ... return optimize(); } -/**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { - GaussianFactorGraph::shared_ptr Ab1 = - boost::make_shared(), Ab2 = boost::make_shared< - GaussianFactorGraph>(); - - boost::tie(Ab1, Ab2) = splitGraph(jfg); - if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() - << " factors" << endl; - - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, - EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared( - Rc1->optimize()); - pc_ = boost::make_shared(Ab2, Rc1, xbar); +VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { + return VectorValues(); } - -/**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2) { - VectorValues::shared_ptr xbar = boost::make_shared( - Rc1->optimize()); - pc_ = boost::make_shared(Ab2, Rc1, xbar); -} - /**************************************************************************************************/ // Run Kruskal algorithm to create a spanning tree of factor "edges". // Edges are not weighted, and will only work if factors are binary. @@ -163,9 +143,5 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) { } /****************************************************************************/ -VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial) { - return VectorValues(); -} + } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 44308ca1c..099982b53 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -70,7 +70,6 @@ public: protected: Parameters parameters_; - Ordering ordering_; boost::shared_ptr pc_; ///< preconditioner object public: @@ -88,8 +87,8 @@ public: const Parameters ¶meters, const Ordering& ordering); /** - * The user specify the subgraph part and the constraint part - * may throw exception if A1 is underdetermined + * The user specifies the subgraph part and the constraints part. + * May throw exception if A1 is underdetermined. An ordering is required to eliminate Ab1. */ SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); @@ -99,15 +98,14 @@ public: const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); - /* The same as above, but the A1 is solved before */ + /// The same as above, but we assume A1 was solved by caller SubgraphSolver(const boost::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering); + const GaussianFactorGraph &Ab2, const Parameters ¶meters); /// Shared pointer version SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, - const Parameters ¶meters, const Ordering& ordering); + const Parameters ¶meters); /// Destructor virtual ~SubgraphSolver() { @@ -125,13 +123,8 @@ public: const VectorValues &initial); protected: - - void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2); - - boost::tuple, - boost::shared_ptr > + /// Split graph using Kruskal algorithm, treating binary factors as edges. + static boost::tuple, boost::shared_ptr> splitGraph(const GaussianFactorGraph &gfg); };