diff --git a/cpp/SubgraphPreconditioner-inl.h b/cpp/SubgraphPreconditioner-inl.h index f3d42ed3b..cac25f24c 100644 --- a/cpp/SubgraphPreconditioner-inl.h +++ b/cpp/SubgraphPreconditioner-inl.h @@ -29,12 +29,20 @@ namespace gtsam { list keys = predecessorMap2Keys(tree); // split the graph - Key root = keys.back(); if (verbose_) cout << "generating spanning tree and split the graph ..."; G.template split(tree, T_, C_); if (verbose_) cout << T_.size() << " and " << C_.size() << " factors" << endl; - // theta_bar = composePoses (T_, tree, config[root]); + // make the ordering + list symbols; + symbols.resize(keys.size()); + std::transform(keys.begin(), keys.end(), symbols.begin(), key2symbol); + ordering_ = boost::shared_ptr(new Ordering(symbols)); + + // compose the approximate solution + Key root = keys.back(); + theta_bar_ = composePoses (T_, tree, config[root]); + } /* ************************************************************************* */ @@ -43,7 +51,7 @@ namespace gtsam { const Config& theta_bar, const Ordering& ordering) const { VectorConfig zeros; - BOOST_FOREACH(const string& j, ordering) zeros.insert(j,zero(3)); + BOOST_FOREACH(const Symbol& j, ordering) zeros.insert(j,zero(3)); // build the subgraph PCG system GaussianFactorGraph Ab1 = T_.linearize(theta_bar); diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index b26fea3d2..6f5d1b7e6 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/cpp/SubgraphPreconditioner.h @@ -9,6 +9,7 @@ #include "GaussianFactorGraph.h" #include "GaussianBayesNet.h" +#include "Ordering.h" namespace gtsam { @@ -59,8 +60,6 @@ namespace gtsam { void print(const std::string& s = "SubgraphPreconditioner") const; }; - class Ordering; - /** * A linear system solver using subgraph preconditioning conjugate gradient */ @@ -76,12 +75,24 @@ namespace gtsam { const bool verbose_; const double epsilon_, epsilon_abs_; + /* the ordering derived from the spanning tree */ + boost::shared_ptr ordering_; + + /* the solution computed from the first subgraph */ + boost::shared_ptr theta_bar_; + NonlinearGraph T_, C_; public: - SubgraphPCG() {} + // kai: this constructor is for compatible with Factorization + SubgraphPCG() { throw std::runtime_error("SubgraphPCG: this constructor is only for compatibility!");} + SubgraphPCG(const NonlinearGraph& G, const Config& config); + boost::shared_ptr ordering() const { return ordering_; } + + boost::shared_ptr theta_bar() const { return theta_bar_; } + /** * solve for the optimal displacement in the tangent space, and then solve * the resulted linear system