From 485175e2f89e71c8dd858e7a7683aae75cb85906 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Apr 2019 01:04:18 -0400 Subject: [PATCH 1/6] Fixed major bug: splitting off a subgraph preconditioner only worked if keys were numbered 0...n-1, because we used DSFVector to implement Kruskal. Now it'll be a bit slower but will work for any keys. Note this only affected two constructors. --- gtsam/linear/SubgraphSolver.cpp | 57 +++++++++++++++++---------------- gtsam/linear/SubgraphSolver.h | 6 +++- 2 files changed, 34 insertions(+), 29 deletions(-) diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 22d39a7f2..3853a72fa 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -22,7 +22,9 @@ #include #include #include -#include +#include + +#include using namespace std; @@ -120,45 +122,44 @@ void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, } /**************************************************************************************************/ +// Run Kruskal algorithm to create a spanning tree of factor "edges". +// Edges are not weighted, and will only work if factors are binary. +// Unary factors are ignored for this purpose and added to tree graph. boost::tuple // -SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { +SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) { - const VariableIndex index(jfg); - const size_t n = index.size(); - DSFVector D(n); + // Create disjoint set forest data structure for Kruskal algorithm + DSFMap dsf; - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); + // Allocate two output graphs + auto tree = boost::make_shared(); + auto constraints = boost::make_shared(); - size_t t = 0; - for ( const GaussianFactor::shared_ptr &gf: jfg ) { + // Loop over all "edges" + for ( const auto &factor: factorGraph ) { - if (gf->keys().size() > 2) { + // Fail on > binary factors + const auto& keys = factor->keys(); + if (keys.size() > 2) { throw runtime_error( "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } - bool augment = false; - - /* check whether this factor should be augmented to the "tree" graph */ - if (gf->keys().size() == 1) - augment = true; - else { - const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.find(u), - v_root = D.find(v); - if (u_root != v_root) { - t++; - augment = true; - D.merge(u_root, v_root); - } + // check whether this factor should be augmented to the "tree" graph + if (keys.size() == 1) + tree->push_back(factor); + else if (dsf.find(keys[0]) != dsf.find(keys[1])) { + // We merge two trees joined by this edge if they are still disjoint + tree->push_back(factor); + // Record this fact in DSF + dsf.merge(keys[0], keys[1]); + } else { + // This factor would create a loop, so we add it to non-tree edges... + constraints->push_back(factor); } - if (augment) - At->push_back(gf); - else - Ac->push_back(gf); } - return boost::tie(At, Ac); + return boost::tie(tree, constraints); } /****************************************************************************/ diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 318c029f3..44308ca1c 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -75,7 +75,11 @@ protected: public: - /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + /** + * Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + * Will throw exception if there are ternary factors or higher arity, as we use Kruskal's + * algorithm to split the graph, treating binary factors as edges. + */ SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering& ordering); From 6d938ce5cc94dbb24643f80eeb9c4a7a199ec491 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Apr 2019 01:17:28 -0400 Subject: [PATCH 2/6] Replaced initialize calls with C++11 delegating constructors --- gtsam/linear/SubgraphSolver.cpp | 118 +++++++++++++------------------- gtsam/linear/SubgraphSolver.h | 21 ++---- 2 files changed, 54 insertions(+), 85 deletions(-) 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); }; From aaf2ff568903e655b60637dd56aaa066ce8cdd8e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Apr 2019 01:17:47 -0400 Subject: [PATCH 3/6] Resurrected tests --- tests/testSubgraphSolver.cpp | 51 ++++++++++++++++-------------------- 1 file changed, 22 insertions(+), 29 deletions(-) diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index aeeed1b9f..0a2a1788d 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -15,26 +15,27 @@ * @author Yong-Dian Jian **/ -#include - -#if 0 - #include -#include #include #include #include #include +#include #include #include +#include + #include #include using namespace boost::assign; using namespace std; using namespace gtsam; -using namespace example; + +static size_t N = 3; +static SubgraphSolverParameters kParameters; +static auto kOrdering = example::planarOrdering(N); /* ************************************************************************* */ /** unnormalized error */ @@ -45,20 +46,17 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { return total_error; } - /* ************************************************************************* */ TEST( SubgraphSolver, constructor1 ) { // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // The first constructor just takes a factor graph (and parameters) + // The first constructor just takes a factor graph (and kParameters) // and it will split the graph into A1 and A2, where A1 is a spanning tree - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab, parameters); + SubgraphSolver solver(Ab, kParameters, kOrdering); VectorValues optimized = solver.optimize(); // does PCG optimization DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -70,16 +68,15 @@ TEST( SubgraphSolver, constructor2 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering + // Get the spanning tree GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab); - // The second constructor takes two factor graphs, - // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab1_, Ab2_, parameters); + // The second constructor takes two factor graphs, so the caller can specify + // the preconditioner (Ab1) and the constraints that are left out (Ab2) + SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -91,26 +88,22 @@ TEST( SubgraphSolver, constructor3 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering + // Get the spanning tree and corresponding kOrdering GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab); - // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT - GaussianBayesNet::shared_ptr Rc1 = // - EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT + auto Rc1 = Ab1_.eliminateSequential(); // The third constructor allows the caller to pass an already solved preconditioner Rc1_ // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before - SubgraphSolverParameters parameters; - SubgraphSolver solver(Rc1, Ab2_, parameters); + SubgraphSolver solver(Rc1, Ab2_, kParameters); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 140c666c417e7b7a47ebe3a0dc59302f264b00f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Apr 2019 08:27:28 -0400 Subject: [PATCH 4/6] Moved DSFMap to gtsam --- {gtsam_unstable => gtsam}/base/DSFMap.h | 0 {gtsam_unstable => gtsam}/base/tests/testDSFMap.cpp | 3 +-- gtsam/linear/SubgraphSolver.cpp | 2 +- gtsam_unstable/timing/timeDSFvariants.cpp | 2 +- 4 files changed, 3 insertions(+), 4 deletions(-) rename {gtsam_unstable => gtsam}/base/DSFMap.h (100%) rename {gtsam_unstable => gtsam}/base/tests/testDSFMap.cpp (97%) diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam/base/DSFMap.h similarity index 100% rename from gtsam_unstable/base/DSFMap.h rename to gtsam/base/DSFMap.h diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam/base/tests/testDSFMap.cpp similarity index 97% rename from gtsam_unstable/base/tests/testDSFMap.cpp rename to gtsam/base/tests/testDSFMap.cpp index 9c9143a15..5ffa0d12a 100644 --- a/gtsam_unstable/base/tests/testDSFMap.cpp +++ b/gtsam/base/tests/testDSFMap.cpp @@ -16,7 +16,7 @@ * @brief unit tests for DSFMap */ -#include +#include #include #include @@ -115,7 +115,6 @@ TEST(DSFMap, mergePairwiseMatches2) { TEST(DSFMap, sets){ // Create some "matches" typedef pair Match; - typedef pair > key_pair; list matches; matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 406109758..e7ff38ca5 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index f39bdda59..378d2b572 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include From 3737474d1e760ed16c5b9a40d33a5d0689f5b628 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Apr 2019 09:23:02 -0400 Subject: [PATCH 5/6] Deprecated all but three constructors. --- gtsam/linear/SubgraphSolver.cpp | 48 +++++------ gtsam/linear/SubgraphSolver.h | 140 ++++++++++++++++++-------------- tests/smallExample.h | 12 +-- tests/testSubgraphSolver.cpp | 21 +++-- 4 files changed, 120 insertions(+), 101 deletions(-) diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index e7ff38ca5..edf39e462 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -31,12 +31,12 @@ using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, +// Just taking system [A|b] +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab, const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters) { - GaussianFactorGraph::shared_ptr Ab1,Ab2; - boost::tie(Ab1, Ab2) = splitGraph(gfg); + boost::tie(Ab1, Ab2) = splitGraph(Ab); if (parameters_.verbosity()) cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; @@ -46,12 +46,8 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, pc_ = boost::make_shared(Ab2, Rc1, xbar); } -// delegate up -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &factorGraph, - const Parameters ¶meters, const Ordering& ordering) : - SubgraphSolver(*factorGraph, parameters, ordering) {} - /**************************************************************************************************/ +// Taking eliminated tree [R1|c] and constraint graph [A2|b2] SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters) @@ -60,42 +56,40 @@ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, pc_ = boost::make_shared(Ab2, Rc1, xbar); } +/**************************************************************************************************/ +// Taking subgraphs [A1|b1] and [A2|b2] // delegate up +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph::shared_ptr &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2, + parameters) {} + +/**************************************************************************************************/ +// deprecated variants +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters) : SubgraphSolver(Rc1, boost::make_shared(Ab2), - parameters_) {} + parameters) {} -// delegate up SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering &ordering) - : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), - boost::make_shared(Ab2), - parameters_) {} - -// delegate up -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, - const Parameters ¶meters, - const Ordering &ordering) - : SubgraphSolver(Ab1->eliminateSequential(ordering, EliminateQR), Ab2, - parameters) {} + : SubgraphSolver(Ab1, boost::make_shared(Ab2), + parameters, ordering) {} +#endif /**************************************************************************************************/ -VectorValues SubgraphSolver::optimize() { +VectorValues SubgraphSolver::optimize() const { VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } -VectorValues SubgraphSolver::optimize(const VectorValues &initial) { - // the initial is ignored in this case ... - return optimize(); -} - VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, const VectorValues &initial) { diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 099982b53..76fe31d32 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -28,30 +28,34 @@ class GaussianFactorGraph; class GaussianBayesNet; class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { -public: +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); - } + SubgraphSolverParameters() : Base() {} + void print() const { Base::print(); } + virtual void print(std::ostream &os) const { Base::print(os); } }; /** - * This class implements the SPCG solver presented in Dellaert et al in IROS'10. + * This class implements the linear SPCG solver presented in Dellaert et al in + * IROS'10. * - * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into - * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. - * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. - * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the + * problem into \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ + * denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. * - * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it - * with the least-squares variation of the conjugate gradient method. + * \f$A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute + * \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. + * + * Then we solve a reparametrized problem + * \f$ f(y) = |y|^2 + |A_c R_t^{-1} y -\bar{b_y}|^2 \f$, + * where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * + * In the matrix form, it is equivalent to solving + * \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. + * We can solve it with the least-squares variation of the conjugate gradient + * method. * * To use it in nonlinear optimization, please see the following example * @@ -63,69 +67,83 @@ public: * * \nosubgrouping */ -class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { - -public: +class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { + public: typedef SubgraphSolverParameters Parameters; -protected: + protected: Parameters parameters_; - boost::shared_ptr pc_; ///< preconditioner object - -public: + boost::shared_ptr pc_; ///< preconditioner object + public: + /// @name Constructors + /// @{ /** - * Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG - * Will throw exception if there are ternary factors or higher arity, as we use Kruskal's - * algorithm to split the graph, treating binary factors as edges. + * Given a gaussian factor graph, split it into a spanning tree (A1) + others + * (A2) for SPCG Will throw exception if there are ternary factors or higher + * arity, as we use Kruskal's algorithm to split the graph, treating binary + * factors as edges. */ SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, - const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &A, - const Parameters ¶meters, const Ordering& ordering); + const Ordering &ordering); /** * The user specifies the subgraph part and the constraints part. - * May throw exception if A1 is underdetermined. An ordering is required to eliminate Ab1. + * May throw exception if A1 is underdetermined. An ordering is required to + * eliminate Ab1. We take Ab1 as a const reference, as it will be transformed + * into Rc1, but take Ab2 as a shared pointer as we need to keep it around. + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering &ordering); + /** + * The same as above, but we assume A1 was solved by caller. + * We take two shared pointers as we keep both around. */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &Ab1, - const boost::shared_ptr &Ab2, - const Parameters ¶meters, const Ordering& ordering); - - /// The same as above, but we assume A1 was solved by caller SubgraphSolver(const boost::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2, - const Parameters ¶meters); + const boost::shared_ptr &Ab2, + const Parameters ¶meters); /// Destructor - virtual ~SubgraphSolver() { - } + virtual ~SubgraphSolver() {} + + /// @} + /// @name Implement interface + /// @{ /// Optimize from zero - VectorValues optimize(); + VectorValues optimize() const; - /// Optimize from given initial values - VectorValues optimize(const VectorValues &initial); - - /** Interface that IterativeSolver subclasses have to implement */ + /// Interface that IterativeSolver subclasses have to implement virtual VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial); + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial) override; -protected: + /// @} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering &ordering) + : SubgraphSolver(*A, parameters, ordering){}; + SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &, + const Parameters &, const Ordering &); + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering &ordering) + : SubgraphSolver(*Ab1, Ab2, parameters, ordering) {} + SubgraphSolver(const boost::shared_ptr &, + const GaussianFactorGraph &, const Parameters &); + /// @} +#endif + + protected: /// Split graph using Kruskal algorithm, treating binary factors as edges. - static boost::tuple, boost::shared_ptr> + static boost::tuple, + boost::shared_ptr> splitGraph(const GaussianFactorGraph &gfg); }; -} // namespace gtsam +} // namespace gtsam diff --git a/tests/smallExample.h b/tests/smallExample.h index d3a69b0bd..8cd219bff 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -614,26 +614,26 @@ inline Ordering planarOrdering(size_t N) { } /* ************************************************************************* */ -inline std::pair splitOffPlanarTree(size_t N, +inline std::pair splitOffPlanarTree(size_t N, const GaussianFactorGraph& original) { - GaussianFactorGraph T, C; + auto T = boost::make_shared(), C= boost::make_shared(); // Add the x11 constraint to the tree - T.push_back(original[0]); + T->push_back(original[0]); // Add all horizontal constraints to the tree size_t i = 1; for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++, i++) - T.push_back(original[i]); + T->push_back(original[i]); // Add first vertical column of constraints to T, others to C for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++, i++) if (x == 1) - T.push_back(original[i]); + T->push_back(original[i]); else - C.push_back(original[i]); + C->push_back(original[i]); return std::make_pair(T, C); } diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 0a2a1788d..93101131d 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -46,6 +46,13 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { return total_error; } +/* ************************************************************************* */ +TEST( SubgraphSolver, Parameters ) +{ + LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity()); + LONGS_EQUAL(500, kParameters.maxIterations()); +} + /* ************************************************************************* */ TEST( SubgraphSolver, constructor1 ) { @@ -71,12 +78,12 @@ TEST( SubgraphSolver, constructor2 ) boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b // Get the spanning tree - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab); + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); // The second constructor takes two factor graphs, so the caller can specify // the preconditioner (Ab1) and the constraints that are left out (Ab2) - SubgraphSolver solver(Ab1_, Ab2_, kParameters, kOrdering); + SubgraphSolver solver(*Ab1, Ab2, kParameters, kOrdering); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -91,15 +98,15 @@ TEST( SubgraphSolver, constructor3 ) boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b // Get the spanning tree and corresponding kOrdering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = example::splitOffPlanarTree(N, Ab); + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT - auto Rc1 = Ab1_.eliminateSequential(); + auto Rc1 = Ab1->eliminateSequential(); // The third constructor allows the caller to pass an already solved preconditioner Rc1_ // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before - SubgraphSolver solver(Rc1, Ab2_, kParameters); + SubgraphSolver solver(Rc1, Ab2, kParameters); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } From 703b56f9ff4d85a566e25f8edaf9d16d47720d25 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 5 Apr 2019 08:10:27 -0400 Subject: [PATCH 6/6] Fix SubgraphSolver wrapper --- gtsam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 014907037..ba3b7f7c7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1730,7 +1730,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { virtual class SubgraphSolver { SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); gtsam::VectorValues optimize() const; };