diff --git a/cpp/SubgraphSolver-inl.h b/cpp/SubgraphSolver-inl.h index 9aff2d13c..8daa1eac8 100644 --- a/cpp/SubgraphSolver-inl.h +++ b/cpp/SubgraphSolver-inl.h @@ -20,22 +20,22 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - template - SubgraphSolver::SubgraphSolver(const G& g, const T& theta0) { - initialize(g,theta0); + template + SubgraphSolver::SubgraphSolver(const Graph& G, const Config& theta0) { + initialize(G,theta0); } /* ************************************************************************* */ - template - void SubgraphSolver::initialize(const G& g, const T& theta0) { + template + void SubgraphSolver::initialize(const Graph& G, const Config& theta0) { // generate spanning tree - PredecessorMap tree = g.template findMinimumSpanningTree(); + PredecessorMap tree = G.template findMinimumSpanningTree(); list keys = predecessorMap2Keys(tree); // split the graph if (verbose_) cout << "generating spanning tree and split the graph ..."; - g.template split(tree, T_, C_); + G.template split(tree, T_, C_); if (verbose_) cout << T_.size() << " and " << C_.size() << " factors" << endl; // make the ordering @@ -46,12 +46,12 @@ namespace gtsam { // compose the approximate solution Key root = keys.back(); - theta_bar_ = composePoses (T_, tree, theta0[root]); + theta_bar_ = composePoses (T_, tree, theta0[root]); } /* ************************************************************************* */ - template - boost::shared_ptr SubgraphSolver::linearize(const G& g, const T& theta_bar) const { + template + boost::shared_ptr SubgraphSolver::linearize(const Graph& G, const Config& theta_bar) const { SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar); SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar); #ifdef TIMING @@ -68,8 +68,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - VectorConfig SubgraphSolver::optimize(SubgraphPreconditioner& system) const { + template + VectorConfig SubgraphSolver::optimize(SubgraphPreconditioner& system) const { VectorConfig zeros = system.zero(); // Solve the subgraph PCG diff --git a/cpp/SubgraphSolver.h b/cpp/SubgraphSolver.h index 45fb72d04..8e9ea565e 100644 --- a/cpp/SubgraphSolver.h +++ b/cpp/SubgraphSolver.h @@ -19,13 +19,13 @@ namespace gtsam { * linearize: G * T -> L * solve : L -> VectorConfig */ - template + template class SubgraphSolver { private: - typedef typename T::Key Key; - typedef typename G::Constraint Constraint; - typedef typename G::Pose Pose; + typedef typename Config::Key Key; + typedef typename Graph::Constraint Constraint; + typedef typename Graph::Pose Pose; // TODO not hardcode static const size_t maxIterations_=100; @@ -36,25 +36,25 @@ namespace gtsam { boost::shared_ptr ordering_; /* the solution computed from the first subgraph */ - boost::shared_ptr theta_bar_; + boost::shared_ptr theta_bar_; - G T_, C_; + Graph T_, C_; public: SubgraphSolver() {} - SubgraphSolver(const G& g, const T& theta0); + SubgraphSolver(const Graph& G, const Config& theta0); - void initialize(const G& g, const T& theta0); + void initialize(const Graph& G, const Config& theta0); boost::shared_ptr ordering() const { return ordering_; } - boost::shared_ptr theta_bar() const { return theta_bar_; } + 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; + boost::shared_ptr linearize(const Graph& G, const Config& theta_bar) const; /** * solve for the optimal displacement in the tangent space, and then solve @@ -63,9 +63,9 @@ namespace gtsam { 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_; + template const size_t SubgraphSolver::maxIterations_; + template const bool SubgraphSolver::verbose_; + template const double SubgraphSolver::epsilon_; + template const double SubgraphSolver::epsilon_abs_; } // nsamespace gtsam