From 3737474d1e760ed16c5b9a40d33a5d0689f5b628 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Apr 2019 09:23:02 -0400 Subject: [PATCH] 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); }