From 00ac961c8a6fd732d532d72cd27962e9a931c316 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Mar 2010 19:19:21 +0000 Subject: [PATCH] I changed the name of SubgraphPCG to SubgraphSolver and put it in its own compilation unit --- cpp/Makefile.am | 2 +- cpp/Pose2SLAMOptimizer.cpp | 2 +- cpp/Pose2SLAMOptimizer.h | 4 +- cpp/SubgraphPreconditioner.h | 62 +--------------- ...conditioner-inl.h => SubgraphSolver-inl.h} | 12 ++-- cpp/SubgraphSolver.h | 71 +++++++++++++++++++ cpp/testNonlinearOptimizer.cpp | 8 +-- 7 files changed, 87 insertions(+), 74 deletions(-) rename cpp/{SubgraphPreconditioner-inl.h => SubgraphSolver-inl.h} (86%) create mode 100644 cpp/SubgraphSolver.h diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 09eee333b..a9f239532 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -115,7 +115,7 @@ testErrors_SOURCES = testErrors.cpp testErrors_LDADD = libgtsam.la # Iterative Methods -headers += iterative-inl.h SubgraphPreconditioner-inl.h +headers += iterative-inl.h SubgraphSolver.h SubgraphSolver-inl.h sources += iterative.cpp BayesNetPreconditioner.cpp SubgraphPreconditioner.cpp check_PROGRAMS += testIterative testBayesNetPreconditioner testSubgraphPreconditioner testIterative_SOURCES = testIterative.cpp diff --git a/cpp/Pose2SLAMOptimizer.cpp b/cpp/Pose2SLAMOptimizer.cpp index 093dc2533..43ff60277 100644 --- a/cpp/Pose2SLAMOptimizer.cpp +++ b/cpp/Pose2SLAMOptimizer.cpp @@ -8,7 +8,7 @@ #include "Pose2SLAMOptimizer.h" #include "pose2SLAM.h" #include "dataset.h" -#include "SubgraphPreconditioner-inl.h" +#include "SubgraphSolver-inl.h" using namespace std; namespace gtsam { diff --git a/cpp/Pose2SLAMOptimizer.h b/cpp/Pose2SLAMOptimizer.h index 70720bd9d..af1d2dc1f 100644 --- a/cpp/Pose2SLAMOptimizer.h +++ b/cpp/Pose2SLAMOptimizer.h @@ -12,7 +12,7 @@ #include "pose2SLAM.h" #include "Ordering.h" -#include "SubgraphPreconditioner.h" +#include "SubgraphSolver.h" namespace gtsam { @@ -31,7 +31,7 @@ namespace gtsam { boost::shared_ptr theta_; /** Non-linear solver */ - typedef SubgraphPCG SPCG_Solver; + typedef SubgraphSolver SPCG_Solver; SPCG_Solver solver_; /** Linear Solver */ diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index 8fe158e61..995e2ca79 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/cpp/SubgraphPreconditioner.h @@ -4,8 +4,7 @@ * @author: Frank Dellaert */ -#ifndef SUBGRAPHPRECONDITIONER_H_ -#define SUBGRAPHPRECONDITIONER_H_ +#pragma once #include "GaussianFactorGraph.h" #include "GaussianBayesNet.h" @@ -91,61 +90,4 @@ namespace gtsam { void print(const std::string& s = "SubgraphPreconditioner") const; }; - /** - * A nonlinear system solver using subgraph preconditioning conjugate gradient - * Concept NonLinearSolver implements - * linearize: G * T -> L - * solve : L -> VectorConfig - */ - template - class SubgraphPCG { - - private: - typedef typename T::Key Key; - typedef typename G::Constraint Constraint; - typedef typename G::Pose Pose; - - // TODO not hardcode - static const size_t maxIterations_=100; - static const bool verbose_=false; - static const double epsilon_=1e-4, epsilon_abs_=1e-5; - - /* the ordering derived from the spanning tree */ - boost::shared_ptr ordering_; - - /* the solution computed from the first subgraph */ - boost::shared_ptr theta_bar_; - - G T_, C_; - - public: - SubgraphPCG() {} - - SubgraphPCG(const G& g, const T& theta0); - - void initialize(const G& g, const T& theta0); - - boost::shared_ptr ordering() const { return ordering_; } - - boost::shared_ptr theta_bar() const { return theta_bar_; } - - /** - * linearize the non-linear graph around the current config and build the subgraph preconditioner systme - */ - boost::shared_ptr linearize(const G& g, const T& theta_bar) const; - - /** - * solve for the optimal displacement in the tangent space, and then solve - * the resulted linear system - */ - VectorConfig optimize(SubgraphPreconditioner& system) const; - }; - - template const size_t SubgraphPCG::maxIterations_; - template const bool SubgraphPCG::verbose_; - template const double SubgraphPCG::epsilon_; - template const double SubgraphPCG::epsilon_abs_; - -} // nsamespace gtsam - -#endif /* SUBGRAPHPRECONDITIONER_H_ */ +} // namespace gtsam diff --git a/cpp/SubgraphPreconditioner-inl.h b/cpp/SubgraphSolver-inl.h similarity index 86% rename from cpp/SubgraphPreconditioner-inl.h rename to cpp/SubgraphSolver-inl.h index bc81b9229..9aff2d13c 100644 --- a/cpp/SubgraphPreconditioner-inl.h +++ b/cpp/SubgraphSolver-inl.h @@ -1,5 +1,5 @@ /* - * SubgraphPreconditioner-inl.h + * SubgraphSolver-inl.h * * Created on: Jan 17, 2010 * Author: nikai @@ -9,7 +9,7 @@ #pragma once #include -#include "SubgraphPreconditioner.h" +#include "SubgraphSolver.h" #include "graph-inl.h" #include "iterative-inl.h" @@ -21,13 +21,13 @@ namespace gtsam { /* ************************************************************************* */ template - SubgraphPCG::SubgraphPCG(const G& g, const T& theta0) { + SubgraphSolver::SubgraphSolver(const G& g, const T& theta0) { initialize(g,theta0); } /* ************************************************************************* */ template - void SubgraphPCG::initialize(const G& g, const T& theta0) { + void SubgraphSolver::initialize(const G& g, const T& theta0) { // generate spanning tree PredecessorMap tree = g.template findMinimumSpanningTree(); @@ -51,7 +51,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr SubgraphPCG::linearize(const G& g, const T& theta_bar) const { + boost::shared_ptr SubgraphSolver::linearize(const G& g, const T& theta_bar) const { SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar); SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar); #ifdef TIMING @@ -69,7 +69,7 @@ namespace gtsam { /* ************************************************************************* */ template - VectorConfig SubgraphPCG::optimize(SubgraphPreconditioner& system) const { + VectorConfig SubgraphSolver::optimize(SubgraphPreconditioner& system) const { VectorConfig zeros = system.zero(); // Solve the subgraph PCG diff --git a/cpp/SubgraphSolver.h b/cpp/SubgraphSolver.h new file mode 100644 index 000000000..45fb72d04 --- /dev/null +++ b/cpp/SubgraphSolver.h @@ -0,0 +1,71 @@ +/* + * SubgraphSolver.h + * Created on: Dec 31, 2009 + * @author: Frank Dellaert + */ + +#pragma once + +#include "GaussianFactorGraph.h" +#include "GaussianBayesNet.h" +#include "Ordering.h" +#include "SubgraphPreconditioner.h" + +namespace gtsam { + + /** + * A nonlinear system solver using subgraph preconditioning conjugate gradient + * Concept NonLinearSolver implements + * linearize: G * T -> L + * solve : L -> VectorConfig + */ + template + class SubgraphSolver { + + private: + typedef typename T::Key Key; + typedef typename G::Constraint Constraint; + typedef typename G::Pose Pose; + + // TODO not hardcode + static const size_t maxIterations_=100; + static const bool verbose_=false; + static const double epsilon_=1e-4, epsilon_abs_=1e-5; + + /* the ordering derived from the spanning tree */ + boost::shared_ptr ordering_; + + /* the solution computed from the first subgraph */ + boost::shared_ptr theta_bar_; + + G T_, C_; + + public: + SubgraphSolver() {} + + SubgraphSolver(const G& g, const T& theta0); + + void initialize(const G& g, const T& theta0); + + boost::shared_ptr ordering() const { return ordering_; } + + boost::shared_ptr theta_bar() const { return theta_bar_; } + + /** + * linearize the non-linear graph around the current config and build the subgraph preconditioner systme + */ + boost::shared_ptr linearize(const G& g, const T& theta_bar) const; + + /** + * solve for the optimal displacement in the tangent space, and then solve + * the resulted linear system + */ + VectorConfig optimize(SubgraphPreconditioner& system) const; + }; + + template const size_t SubgraphSolver::maxIterations_; + template const bool SubgraphSolver::verbose_; + template const double SubgraphSolver::epsilon_; + template const double SubgraphSolver::epsilon_abs_; + +} // nsamespace gtsam diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 4f5379ef2..6a5b57942 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -27,7 +27,7 @@ using namespace boost; // template definitions #include "NonlinearFactorGraph-inl.h" #include "NonlinearOptimizer-inl.h" -#include "SubgraphPreconditioner-inl.h" +#include "SubgraphSolver-inl.h" using namespace gtsam; @@ -202,10 +202,10 @@ TEST( NonlinearOptimizer, Factorization ) } /* ************************************************************************* */ -TEST( NonlinearOptimizer, SubgraphPCG ) +TEST( NonlinearOptimizer, SubgraphSolver ) { using namespace pose2SLAM; - typedef SubgraphPCG Solver; + typedef SubgraphSolver Solver; typedef NonlinearOptimizer Optimizer; // Create a graph @@ -220,7 +220,7 @@ TEST( NonlinearOptimizer, SubgraphPCG ) // Create solver and optimizer Optimizer::shared_solver solver - (new SubgraphPCG (*graph, *config)); + (new SubgraphSolver (*graph, *config)); Optimizer optimizer(graph, config, solver); // Optimize !!!!