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; }; 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 22d39a7f2..edf39e462 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -22,149 +22,120 @@ #include #include #include -#include +#include + +#include 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), ordering_(ordering) { - initialize(gfg); + parameters_(parameters) { + GaussianFactorGraph::shared_ptr Ab1,Ab2; + boost::tie(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 = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, - const Parameters ¶meters, const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(*jfg); +// 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) + : parameters_(parameters) { + auto xbar = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ +// Taking subgraphs [A1|b1] and [A2|b2] +// delegate up SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, - EliminateQR); - initialize(Rc1, boost::make_shared(Ab2)); -} - -/**************************************************************************************************/ -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); -} + 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, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(Rc1, boost::make_shared(Ab2)); -} + const GaussianFactorGraph &Ab2, + const Parameters ¶meters) + : SubgraphSolver(Rc1, boost::make_shared(Ab2), + parameters) {} + +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1, boost::make_shared(Ab2), + parameters, ordering) {} +#endif /**************************************************************************************************/ -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); -} - -/**************************************************************************************************/ -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(); -} - -/**************************************************************************************************/ -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); -} - -/**************************************************************************************************/ -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); -} - -/**************************************************************************************************/ -boost::tuple // -SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { - - const VariableIndex index(jfg); - const size_t n = index.size(); - DSFVector D(n); - - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); - - size_t t = 0; - for ( const GaussianFactor::shared_ptr &gf: jfg ) { - - if (gf->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); - } - } - if (augment) - At->push_back(gf); - else - Ac->push_back(gf); - } - - return boost::tie(At, Ac); -} - -/****************************************************************************/ VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, const VectorValues &initial) { return VectorValues(); } +/**************************************************************************************************/ +// 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 &factorGraph) { + + // Create disjoint set forest data structure for Kruskal algorithm + DSFMap dsf; + + // Allocate two output graphs + auto tree = boost::make_shared(); + auto constraints = boost::make_shared(); + + // Loop over all "edges" + for ( const auto &factor: factorGraph ) { + + // 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 "); + } + + // 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); + } + } + + return boost::tie(tree, constraints); +} + +/****************************************************************************/ + } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 318c029f3..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,72 +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_; - Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object -public: - - /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + 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. + */ 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 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. 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 the A1 is solved before */ SubgraphSolver(const boost::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2, - const Parameters ¶meters, const Ordering& ordering); + 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: + /// @} - void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2); +#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 - boost::tuple, - boost::shared_ptr > + protected: + /// Split graph using Kruskal algorithm, treating binary factors as edges. + static boost::tuple, + boost::shared_ptr> splitGraph(const GaussianFactorGraph &gfg); }; -} // namespace gtsam +} // namespace gtsam 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 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 aeeed1b9f..93101131d 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,6 +46,12 @@ 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 ) @@ -52,13 +59,11 @@ 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 +75,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 - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + // Get the spanning tree + 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) - 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 +95,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 - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + // Get the spanning tree and corresponding kOrdering + 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 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); } /* ************************************************************************* */