diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp new file mode 100644 index 000000000..22ad89cd8 --- /dev/null +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -0,0 +1,545 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SubgraphBuilder.cpp + * @date Dec 31, 2009 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include // accumulate +#include +#include +#include +#include + +using std::cout; +using std::endl; +using std::ostream; +using std::vector; + +namespace gtsam { + +/*****************************************************************************/ +/* sort the container and return permutation index with default comparator */ +template +static vector sort_idx(const Container &src) { + typedef typename Container::value_type T; + const size_t n = src.size(); + vector > tmp; + tmp.reserve(n); + for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]); + + /* sort */ + std::stable_sort(tmp.begin(), tmp.end()); + + /* copy back */ + vector idx; + idx.reserve(n); + for (size_t i = 0; i < n; i++) { + idx.push_back(tmp[i].first); + } + return idx; +} + +/*****************************************************************************/ +static vector iidSampler(const vector &weight, const size_t n) { + /* compute the sum of the weights */ + const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); + if (sum == 0.0) { + throw std::runtime_error("Weight vector has no non-zero weights"); + } + + /* make a normalized and accumulated version of the weight vector */ + const size_t m = weight.size(); + vector cdf; + cdf.reserve(m); + for (size_t i = 0; i < m; ++i) { + cdf.push_back(weight[i] / sum); + } + + vector acc(m); + std::partial_sum(cdf.begin(), cdf.end(), acc.begin()); + + /* iid sample n times */ + vector samples; + samples.reserve(n); + const double denominator = (double)RAND_MAX; + for (size_t i = 0; i < n; ++i) { + const double value = rand() / denominator; + /* binary search the interval containing "value" */ + const auto it = std::lower_bound(acc.begin(), acc.end(), value); + const size_t index = it - acc.begin(); + samples.push_back(index); + } + return samples; +} + +/*****************************************************************************/ +static vector UniqueSampler(const vector &weight, + const size_t n) { + const size_t m = weight.size(); + if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size"); + + vector samples; + + size_t count = 0; + vector touched(m, false); + while (count < n) { + vector localIndices; + localIndices.reserve(n - count); + vector localWeights; + localWeights.reserve(n - count); + + /* collect data */ + for (size_t i = 0; i < m; ++i) { + if (!touched[i]) { + localIndices.push_back(i); + localWeights.push_back(weight[i]); + } + } + + /* sampling and cache results */ + vector samples = iidSampler(localWeights, n - count); + for (const size_t &index : samples) { + if (touched[index] == false) { + touched[index] = true; + samples.push_back(index); + if (++count >= n) break; + } + } + } + return samples; +} + +/****************************************************************************/ +Subgraph::Subgraph(const vector &indices) { + edges_.reserve(indices.size()); + for (const size_t &index : indices) { + const Edge e {index,1.0}; + edges_.push_back(e); + } +} + +/****************************************************************************/ +vector Subgraph::edgeIndices() const { + vector eid; + eid.reserve(size()); + for (const Edge &edge : edges_) { + eid.push_back(edge.index); + } + return eid; +} + +/****************************************************************************/ +void Subgraph::save(const std::string &fn) const { + std::ofstream os(fn.c_str()); + boost::archive::text_oarchive oa(os); + oa << *this; + os.close(); +} + +/****************************************************************************/ +Subgraph Subgraph::load(const std::string &fn) { + std::ifstream is(fn.c_str()); + boost::archive::text_iarchive ia(is); + Subgraph subgraph; + ia >> subgraph; + is.close(); + return subgraph; +} + +/****************************************************************************/ +ostream &operator<<(ostream &os, const Subgraph::Edge &edge) { + if (edge.weight != 1.0) + os << edge.index << "(" << std::setprecision(2) << edge.weight << ")"; + else + os << edge.index; + return os; +} + +/****************************************************************************/ +ostream &operator<<(ostream &os, const Subgraph &subgraph) { + os << "Subgraph" << endl; + for (const auto &e : subgraph.edges()) { + os << e << ", "; + } + return os; +} + +/*****************************************************************************/ +void SubgraphBuilderParameters::print() const { print(cout); } + +/***************************************************************************************/ +void SubgraphBuilderParameters::print(ostream &os) const { + os << "SubgraphBuilderParameters" << endl + << "skeleton: " << skeletonTranslator(skeletonType) << endl + << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight) + << endl + << "augmentation weight: " + << augmentationWeightTranslator(augmentationWeight) << endl; +} + +/*****************************************************************************/ +ostream &operator<<(ostream &os, const SubgraphBuilderParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +SubgraphBuilderParameters::Skeleton +SubgraphBuilderParameters::skeletonTranslator(const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "NATURALCHAIN") + return NATURALCHAIN; + else if (s == "BFS") + return BFS; + else if (s == "KRUSKAL") + return KRUSKAL; + throw std::invalid_argument( + "SubgraphBuilderParameters::skeletonTranslator undefined string " + s); + return KRUSKAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) { + if (s == NATURALCHAIN) + return "NATURALCHAIN"; + else if (s == BFS) + return "BFS"; + else if (s == KRUSKAL) + return "KRUSKAL"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::SkeletonWeight +SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "EQUAL") + return EQUAL; + else if (s == "RHS") + return RHS_2NORM; + else if (s == "LHS") + return LHS_FNORM; + else if (s == "RANDOM") + return RANDOM; + throw std::invalid_argument( + "SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + + s); + return EQUAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonWeightTranslator( + SkeletonWeight w) { + if (w == EQUAL) + return "EQUAL"; + else if (w == RHS_2NORM) + return "RHS"; + else if (w == LHS_FNORM) + return "LHS"; + else if (w == RANDOM) + return "RANDOM"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::AugmentationWeight +SubgraphBuilderParameters::augmentationWeightTranslator( + const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "SKELETON") return SKELETON; + // else if (s == "STRETCH") return STRETCH; + // else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; + throw std::invalid_argument( + "SubgraphBuilder::Parameters::augmentationWeightTranslator undefined " + "string " + + s); + return SKELETON; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::augmentationWeightTranslator( + AugmentationWeight w) { + if (w == SKELETON) return "SKELETON"; + // else if ( w == STRETCH ) return "STRETCH"; + // else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &weights) const { + const SubgraphBuilderParameters &p = parameters_; + switch (p.skeletonType) { + case SubgraphBuilderParameters::NATURALCHAIN: + return natural_chain(gfg); + break; + case SubgraphBuilderParameters::BFS: + return bfs(gfg); + break; + case SubgraphBuilderParameters::KRUSKAL: + return kruskal(gfg, ordering, weights); + break; + default: + std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; + break; + } + return vector(); +} + +/****************************************************************/ +vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { + vector unaryFactorIndices; + size_t index = 0; + for (const auto &factor : gfg) { + if (factor->size() == 1) { + unaryFactorIndices.push_back(index); + } + index++; + } + return unaryFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::natural_chain( + const GaussianFactorGraph &gfg) const { + vector chainFactorIndices; + size_t index = 0; + for (const GaussianFactor::shared_ptr &gf : gfg) { + if (gf->size() == 2) { + const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; + if ((k1 - k0) == 1 || (k0 - k1) == 1) chainFactorIndices.push_back(index); + } + index++; + } + return chainFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { + const VariableIndex variableIndex(gfg); + /* start from the first key of the first factor */ + Key seed = gfg[0]->keys()[0]; + + const size_t n = variableIndex.size(); + + /* each vertex has self as the predecessor */ + vector bfsFactorIndices; + bfsFactorIndices.reserve(n - 1); + + /* Initialize */ + std::queue q; + q.push(seed); + + std::set flags; + flags.insert(seed); + + /* traversal */ + while (!q.empty()) { + const size_t head = q.front(); + q.pop(); + for (const size_t index : variableIndex[head]) { + const GaussianFactor &gf = *gfg[index]; + for (const size_t key : gf.keys()) { + if (flags.count(key) == 0) { + q.push(key); + flags.insert(key); + bfsFactorIndices.push_back(index); + } + } + } + } + return bfsFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &weights) const { + const VariableIndex variableIndex(gfg); + const size_t n = variableIndex.size(); + const vector sortedIndices = sort_idx(weights); + + /* initialize buffer */ + vector treeIndices; + treeIndices.reserve(n - 1); + + // container for acsendingly sorted edges + DSFVector dsf(n); + + size_t count = 0; + double sum = 0.0; + for (const size_t index : sortedIndices) { + const GaussianFactor &gf = *gfg[index]; + const auto keys = gf.keys(); + if (keys.size() != 2) continue; + const size_t u = ordering.find(keys[0])->second, + v = ordering.find(keys[1])->second; + if (dsf.find(u) != dsf.find(v)) { + dsf.merge(u, v); + treeIndices.push_back(index); + sum += weights[index]; + if (++count == n - 1) break; + } + } + return treeIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::sample(const vector &weights, + const size_t t) const { + return UniqueSampler(weights, t); +} + +/****************************************************************/ +Subgraph SubgraphBuilder::operator()( + const GaussianFactorGraph &gfg) const { + const auto &p = parameters_; + const auto inverse_ordering = Ordering::Natural(gfg); + const FastMap forward_ordering = inverse_ordering.invert(); + const size_t n = inverse_ordering.size(), m = gfg.size(); + + // Make sure the subgraph preconditioner does not include more than half of + // the edges beyond the spanning tree, or we might as well solve the whole + // thing. + size_t numExtraEdges = n * p.augmentationFactor; + const size_t numRemaining = m - (n - 1); + numExtraEdges = std::min(numExtraEdges, numRemaining / 2); + + // Calculate weights + vector weights = this->weights(gfg); + + // Build spanning tree. + const vector tree = buildTree(gfg, forward_ordering, weights); + if (tree.size() != n - 1) { + throw std::runtime_error( + "SubgraphBuilder::operator() failure: tree.size() != n-1"); + } + + // Downweight the tree edges to zero. + for (const size_t index : tree) { + weights[index] = 0.0; + } + + /* decide how many edges to augment */ + vector offTree = sample(weights, numExtraEdges); + + vector subgraph = unary(gfg); + subgraph.insert(subgraph.end(), tree.begin(), tree.end()); + subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); + + return Subgraph(subgraph); +} + +/****************************************************************/ +SubgraphBuilder::Weights SubgraphBuilder::weights( + const GaussianFactorGraph &gfg) const { + const size_t m = gfg.size(); + Weights weight; + weight.reserve(m); + + for (const GaussianFactor::shared_ptr &gf : gfg) { + switch (parameters_.skeletonWeight) { + case SubgraphBuilderParameters::EQUAL: + weight.push_back(1.0); + break; + case SubgraphBuilderParameters::RHS_2NORM: { + if (JacobianFactor::shared_ptr jf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(jf->getb().norm()); + } else if (HessianFactor::shared_ptr hf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(hf->linearTerm().norm()); + } + } break; + case SubgraphBuilderParameters::LHS_FNORM: { + if (JacobianFactor::shared_ptr jf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(std::sqrt(jf->getA().squaredNorm())); + } else if (HessianFactor::shared_ptr hf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(std::sqrt(hf->information().squaredNorm())); + } + } break; + + case SubgraphBuilderParameters::RANDOM: + weight.push_back(std::rand() % 100 + 1.0); + break; + + default: + throw std::invalid_argument( + "SubgraphBuilder::weights: undefined weight scheme "); + break; + } + } + return weight; +} + +/*****************************************************************************/ +GaussianFactorGraph::shared_ptr buildFactorSubgraph( + const GaussianFactorGraph &gfg, const Subgraph &subgraph, + const bool clone) { + auto subgraphFactors = boost::make_shared(); + subgraphFactors->reserve(subgraph.size()); + for (const auto &e : subgraph) { + const auto factor = gfg[e.index]; + subgraphFactors->push_back(clone ? factor->clone(): factor); + } + return subgraphFactors; +} + +/**************************************************************************************************/ +std::pair // +splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) { + + // Get the subgraph by calling cheaper method + auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false); + + // Now, copy all factors then set subGraph factors to zero + auto remaining = boost::make_shared(factorGraph); + + for (const auto &e : subgraph) { + remaining->remove(e.index); + } + + return std::make_pair(subgraphFactors, remaining); +} + +/*****************************************************************************/ + +} // namespace gtsam diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h new file mode 100644 index 000000000..5247ea2a2 --- /dev/null +++ b/gtsam/linear/SubgraphBuilder.h @@ -0,0 +1,182 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SubgraphBuilder.h + * @date Dec 31, 2009 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include + +#include +#include + +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ + +namespace gtsam { + +// Forward declarations +class GaussianFactorGraph; +struct PreconditionerParameters; + +/**************************************************************************/ +class GTSAM_EXPORT Subgraph { + public: + struct GTSAM_EXPORT Edge { + size_t index; /* edge id */ + double weight; /* edge weight */ + inline bool isUnitWeight() const { return (weight == 1.0); } + friend std::ostream &operator<<(std::ostream &os, const Edge &edge); + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(index); + ar &BOOST_SERIALIZATION_NVP(weight); + } + }; + + typedef std::vector Edges; + typedef std::vector EdgeIndices; + typedef Edges::iterator iterator; + typedef Edges::const_iterator const_iterator; + + protected: + Edges edges_; /* index to the factors */ + + public: + Subgraph() {} + Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} + Subgraph(const Edges &edges) : edges_(edges) {} + Subgraph(const std::vector &indices); + + inline const Edges &edges() const { return edges_; } + inline size_t size() const { return edges_.size(); } + EdgeIndices edgeIndices() const; + + iterator begin() { return edges_.begin(); } + const_iterator begin() const { return edges_.begin(); } + iterator end() { return edges_.end(); } + const_iterator end() const { return edges_.end(); } + + void save(const std::string &fn) const; + static Subgraph load(const std::string &fn); + friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(edges_); + } +}; + +/****************************************************************************/ +struct GTSAM_EXPORT SubgraphBuilderParameters { + typedef boost::shared_ptr shared_ptr; + + enum Skeleton { + /* augmented tree */ + NATURALCHAIN = 0, /* natural ordering of the graph */ + BFS, /* breadth-first search tree */ + KRUSKAL, /* maximum weighted spanning tree */ + } skeletonType; + + enum SkeletonWeight { /* how to weigh the graph edges */ + EQUAL = 0, /* every block edge has equal weight */ + RHS_2NORM, /* use the 2-norm of the rhs */ + LHS_FNORM, /* use the frobenius norm of the lhs */ + RANDOM, /* bounded random edge weight */ + } skeletonWeight; + + enum AugmentationWeight { /* how to weigh the graph edges */ + SKELETON = 0, /* use the same weights in building + the skeleton */ + // STRETCH, /* stretch in the + // laplacian sense */ GENERALIZED_STRETCH /* + // the generalized stretch defined in + // jian2013iros */ + } augmentationWeight; + + /// factor multiplied with n, yields number of extra edges. + double augmentationFactor; + + SubgraphBuilderParameters() + : skeletonType(KRUSKAL), + skeletonWeight(RANDOM), + augmentationWeight(SKELETON), + augmentationFactor(1.0) {} + virtual ~SubgraphBuilderParameters() {} + + /* for serialization */ + void print() const; + virtual void print(std::ostream &os) const; + friend std::ostream &operator<<(std::ostream &os, + const PreconditionerParameters &p); + + static Skeleton skeletonTranslator(const std::string &s); + static std::string skeletonTranslator(Skeleton s); + static SkeletonWeight skeletonWeightTranslator(const std::string &s); + static std::string skeletonWeightTranslator(SkeletonWeight w); + static AugmentationWeight augmentationWeightTranslator(const std::string &s); + static std::string augmentationWeightTranslator(AugmentationWeight w); +}; + +/*****************************************************************************/ +class GTSAM_EXPORT SubgraphBuilder { + public: + typedef SubgraphBuilder Base; + typedef std::vector Weights; + + SubgraphBuilder( + const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : parameters_(p) {} + virtual ~SubgraphBuilder() {} + virtual Subgraph operator()(const GaussianFactorGraph &jfg) const; + + private: + std::vector buildTree(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const std::vector &weights) const; + std::vector unary(const GaussianFactorGraph &gfg) const; + std::vector natural_chain(const GaussianFactorGraph &gfg) const; + std::vector bfs(const GaussianFactorGraph &gfg) const; + std::vector kruskal(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const std::vector &weights) const; + std::vector sample(const std::vector &weights, + const size_t t) const; + Weights weights(const GaussianFactorGraph &gfg) const; + SubgraphBuilderParameters parameters_; +}; + +/** Select the factors in a factor graph according to the subgraph. */ +boost::shared_ptr buildFactorSubgraph( + const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); + +/** Split the graph into a subgraph and the remaining edges. + * Note that the remaining factorgraph has null factors. */ +std::pair, boost::shared_ptr > +splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph); + +} // namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index e011b60d8..d74669c07 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,45 +12,22 @@ /** * @file SubgraphPreconditioner.cpp * @date Dec 31, 2009 - * @author: Frank Dellaert + * @author Frank Dellaert, Yong-Dian Jian */ #include -#include -#include + +#include +#include +#include #include -#include -#include -#include -#include -#include -#include #include #include #include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // accumulate -#include -#include #include -#include -#include -#include using std::cout; using std::endl; @@ -60,422 +37,60 @@ using std::ostream; namespace gtsam { /* ************************************************************************* */ -// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) -static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { +static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, + const KeyVector &keys) { + /* a cache of starting index and dim */ + vector > cache; + cache.reserve(3); + + /* figure out dimension by traversing the keys */ + size_t dim = 0; + for (const Key &key : keys) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + cache.emplace_back(entry.start, entry.dim); + dim += entry.dim; + } + + /* use the cache to fill the result */ + Vector result(dim); + size_t index = 0; + for (const auto &p : cache) { + result.segment(index, p.second) = src.segment(p.first, p.second); + index += p.second; + } + + return result; +} + +/*****************************************************************************/ +static void setSubvector(const Vector &src, const KeyInfo &keyInfo, + const KeyVector &keys, Vector &dst) { + size_t index = 0; + for (const Key &key : keys) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + const size_t keyDim = entry.dim; + dst.segment(entry.start, keyDim) = src.segment(index, keyDim); + index += keyDim; + } +} + +/* ************************************************************************* */ +// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with +// Cholesky) +static GaussianFactorGraph::shared_ptr convertToJacobianFactors( + const GaussianFactorGraph &gfg) { auto result = boost::make_shared(); - for (const auto &factor : gfg) { - auto jf = boost::dynamic_pointer_cast(factor); - if( !jf ) { - jf = boost::make_shared(*factor); - } - result->push_back(jf); - } - return result; -} - -/*****************************************************************************/ -static vector iidSampler(const vector &weight, const size_t n) { - - /* compute the sum of the weights */ - const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); - if (sum==0.0) { - throw std::runtime_error("Weight vector has no non-zero weights"); - } - - /* make a normalized and accumulated version of the weight vector */ - const size_t m = weight.size(); - vector cdf; cdf.reserve(m); - for ( size_t i = 0 ; i < m ; ++i ) { - cdf.push_back(weight[i]/sum); - } - - vector acc(m); - std::partial_sum(cdf.begin(),cdf.end(),acc.begin()); - - /* iid sample n times */ - vector result; result.reserve(n); - const double denominator = (double)RAND_MAX; - for ( size_t i = 0 ; i < n ; ++i ) { - const double value = rand() / denominator; - /* binary search the interval containing "value" */ - const auto it = std::lower_bound(acc.begin(), acc.end(), value); - const size_t index = it - acc.begin(); - result.push_back(index); - } - return result; -} - -/*****************************************************************************/ -static vector UniqueSampler(const vector &weight, const size_t n) { - - const size_t m = weight.size(); - if ( n > m ) throw std::invalid_argument("UniqueSampler: invalid input size"); - - vector result; - - size_t count = 0; - vector touched(m, false); - while ( count < n ) { - vector localIndices; localIndices.reserve(n-count); - vector localWeights; localWeights.reserve(n-count); - - /* collect data */ - for ( size_t i = 0 ; i < m ; ++i ) { - if ( !touched[i] ) { - localIndices.push_back(i); - localWeights.push_back(weight[i]); + for (const auto &factor : gfg) + if (factor) { + auto jf = boost::dynamic_pointer_cast(factor); + if (!jf) { + jf = boost::make_shared(*factor); } + result->push_back(jf); } - - /* sampling and cache results */ - vector samples = iidSampler(localWeights, n-count); - for ( const size_t &index: samples ) { - if ( touched[index] == false ) { - touched[index] = true ; - result.push_back(index); - if ( ++count >= n ) break; - } - } - } return result; } -/****************************************************************************/ -Subgraph::Subgraph(const vector &indices) { - edges_.reserve(indices.size()); - for ( const size_t &index: indices ) { - edges_.emplace_back(index, 1.0); - } -} - -/****************************************************************************/ -vector Subgraph::edgeIndices() const { - vector eid; eid.reserve(size()); - for ( const SubgraphEdge &edge: edges_ ) { - eid.push_back(edge.index_); - } - return eid; -} - -/****************************************************************************/ -void Subgraph::save(const std::string &fn) const { - std::ofstream os(fn.c_str()); - boost::archive::text_oarchive oa(os); - oa << *this; - os.close(); -} - -/****************************************************************************/ -Subgraph::shared_ptr Subgraph::load(const std::string &fn) { - std::ifstream is(fn.c_str()); - boost::archive::text_iarchive ia(is); - Subgraph::shared_ptr subgraph(new Subgraph()); - ia >> *subgraph; - is.close(); - return subgraph; -} - -/****************************************************************************/ -ostream &operator<<(ostream &os, const SubgraphEdge &edge) { - if ( edge.weight() != 1.0 ) - os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; - else - os << edge.index() ; - return os; -} - -/****************************************************************************/ -ostream &operator<<(ostream &os, const Subgraph &subgraph) { - os << "Subgraph" << endl; - for ( const SubgraphEdge &e: subgraph.edges() ) { - os << e << ", " ; - } - return os; -} - -/*****************************************************************************/ -void SubgraphBuilderParameters::print() const { - print(cout); -} - -/***************************************************************************************/ -void SubgraphBuilderParameters::print(ostream &os) const { - os << "SubgraphBuilderParameters" << endl - << "skeleton: " << skeletonTranslator(skeleton_) << endl - << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl - << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl - ; -} - -/*****************************************************************************/ -ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) { - p.print(os); - return os; -} - -/*****************************************************************************/ -SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){ - std::string s = src; boost::algorithm::to_upper(s); - if (s == "NATURALCHAIN") return NATURALCHAIN; - else if (s == "BFS") return BFS; - else if (s == "KRUSKAL") return KRUSKAL; - throw std::invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); - return KRUSKAL; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) { - if ( s == NATURALCHAIN ) return "NATURALCHAIN"; - else if ( s == BFS ) return "BFS"; - else if ( s == KRUSKAL ) return "KRUSKAL"; - else return "UNKNOWN"; -} - -/****************************************************************/ -SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "EQUAL") return EQUAL; - else if (s == "RHS") return RHS_2NORM; - else if (s == "LHS") return LHS_FNORM; - else if (s == "RANDOM") return RANDOM; - throw std::invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); - return EQUAL; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) { - if ( w == EQUAL ) return "EQUAL"; - else if ( w == RHS_2NORM ) return "RHS"; - else if ( w == LHS_FNORM ) return "LHS"; - else if ( w == RANDOM ) return "RANDOM"; - else return "UNKNOWN"; -} - -/****************************************************************/ -SubgraphBuilderParameters::AugmentationWeight -SubgraphBuilderParameters::augmentationWeightTranslator( - const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "SKELETON") return SKELETON; -// else if (s == "STRETCH") return STRETCH; -// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; - throw std::invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); - return SKELETON; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) { - if ( w == SKELETON ) return "SKELETON"; -// else if ( w == STRETCH ) return "STRETCH"; -// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; - else return "UNKNOWN"; -} - -/****************************************************************/ -vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, - const FastMap &ordering, - const vector &weights) const { - const SubgraphBuilderParameters &p = parameters_; - switch (p.skeleton_) { - case SubgraphBuilderParameters::NATURALCHAIN: - return natural_chain(gfg); - break; - case SubgraphBuilderParameters::BFS: - return bfs(gfg); - break; - case SubgraphBuilderParameters::KRUSKAL: - return kruskal(gfg, ordering, weights); - break; - default: - std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; - break; - } - return vector(); -} - -/****************************************************************/ -vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { - vector result ; - size_t index = 0; - for (const auto &factor : gfg) { - if ( factor->size() == 1 ) { - result.push_back(index); - } - index++; - } - return result; -} - -/****************************************************************/ -vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { - vector result ; - size_t index = 0; - for ( const GaussianFactor::shared_ptr &gf: gfg ) { - if ( gf->size() == 2 ) { - const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; - if ( (k1-k0) == 1 || (k0-k1) == 1 ) - result.push_back(index); - } - index++; - } - return result; -} - -/****************************************************************/ -vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { - const VariableIndex variableIndex(gfg); - /* start from the first key of the first factor */ - Key seed = gfg[0]->keys()[0]; - - const size_t n = variableIndex.size(); - - /* each vertex has self as the predecessor */ - vector result; - result.reserve(n-1); - - /* Initialize */ - std::queue q; - q.push(seed); - - std::set flags; - flags.insert(seed); - - /* traversal */ - while ( !q.empty() ) { - const size_t head = q.front(); q.pop(); - for ( const size_t index: variableIndex[head] ) { - const GaussianFactor &gf = *gfg[index]; - for ( const size_t key: gf.keys() ) { - if ( flags.count(key) == 0 ) { - q.push(key); - flags.insert(key); - result.push_back(index); - } - } - } - } - return result; -} - -/****************************************************************/ -vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, - const FastMap &ordering, - const vector &weights) const { - const VariableIndex variableIndex(gfg); - const size_t n = variableIndex.size(); - const vector sortedIndices = sort_idx(weights) ; - - /* initialize buffer */ - vector result; - result.reserve(n-1); - - // container for acsendingly sorted edges - DSFVector dsf(n); - - size_t count = 0 ; double sum = 0.0 ; - for (const size_t index: sortedIndices) { - const GaussianFactor &gf = *gfg[index]; - const auto keys = gf.keys(); - if ( keys.size() != 2 ) continue; - const size_t u = ordering.find(keys[0])->second, - v = ordering.find(keys[1])->second; - if ( dsf.find(u) != dsf.find(v) ) { - dsf.merge(u, v) ; - result.push_back(index) ; - sum += weights[index] ; - if ( ++count == n-1 ) break ; - } - } - return result; -} - -/****************************************************************/ -vector SubgraphBuilder::sample(const vector &weights, const size_t t) const { - return UniqueSampler(weights, t); -} - -/****************************************************************/ -Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { - - const auto &p = parameters_; - const auto inverse_ordering = Ordering::Natural(gfg); - const FastMap forward_ordering = inverse_ordering.invert(); - const size_t n = inverse_ordering.size(), m = gfg.size(); - - // Make sure the subgraph preconditioner does not include more than half of - // the edges beyond the spanning tree, or we might as well solve the whole thing. - size_t numExtraEdges = n * p.complexity_; - const size_t numRemaining = m - (n - 1); - numExtraEdges = std::min(numExtraEdges, numRemaining/2); - - // Calculate weights - vector weights = this->weights(gfg); - - // Build spanning tree. - const vector tree = buildTree(gfg, forward_ordering, weights); - if ( tree.size() != n-1 ) { - throw std::runtime_error("SubgraphBuilder::operator() failure: tree.size() != n-1"); - } - - // Downweight the tree edges to zero. - for ( const size_t index: tree ) { - weights[index] = 0.0; - } - - /* decide how many edges to augment */ - vector offTree = sample(weights, numExtraEdges); - - vector subgraph = unary(gfg); - subgraph.insert(subgraph.end(), tree.begin(), tree.end()); - subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); - - return boost::make_shared(subgraph); -} - -/****************************************************************/ -SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const -{ - const size_t m = gfg.size() ; - Weights weight; weight.reserve(m); - - for(const GaussianFactor::shared_ptr &gf: gfg ) { - switch ( parameters_.skeletonWeight_ ) { - case SubgraphBuilderParameters::EQUAL: - weight.push_back(1.0); - break; - case SubgraphBuilderParameters::RHS_2NORM: - { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(jf->getb().norm()); - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(hf->linearTerm().norm()); - } - } - break; - case SubgraphBuilderParameters::LHS_FNORM: - { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(std::sqrt(jf->getA().squaredNorm())); - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(std::sqrt(hf->information().squaredNorm())); - } - } - break; - - case SubgraphBuilderParameters::RANDOM: - weight.push_back(std::rand()%100 + 1.0); - break; - - default: - throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); - break; - } - } - return weight; -} - /* ************************************************************************* */ SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) : parameters_(p) {} @@ -640,67 +255,18 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const { void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { /* identify the subgraph structure */ - const SubgraphBuilder builder(parameters_.builderParams_); - Subgraph::shared_ptr subgraph = builder(gfg); + const SubgraphBuilder builder(parameters_.builderParams); + auto subgraph = builder(gfg); keyInfo_ = keyInfo; /* build factor subgraph */ - GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true); + GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true); /* factorize and cache BayesNet */ Rc1_ = gfg_subgraph->eliminateSequential(); } /*****************************************************************************/ -Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, - const KeyVector &keys) { - /* a cache of starting index and dim */ - vector > cache; - cache.reserve(3); - - /* figure out dimension by traversing the keys */ - size_t dim = 0; - for (const Key &key : keys) { - const KeyInfoEntry &entry = keyInfo.find(key)->second; - cache.emplace_back(entry.start, entry.dim); - dim += entry.dim; - } - - /* use the cache to fill the result */ - Vector result(dim); - size_t index = 0; - for (const auto &p : cache) { - result.segment(index, p.second) = src.segment(p.first, p.second); - index += p.second; - } - - return result; -} - -/*****************************************************************************/ -void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) { - size_t index = 0; - for ( const Key &key: keys ) { - const KeyInfoEntry &entry = keyInfo.find(key)->second; - const size_t keyDim = entry.dim; - dst.segment(entry.start, keyDim) = src.segment(index, keyDim) ; - index += keyDim; - } -} - -/*****************************************************************************/ -GaussianFactorGraph::shared_ptr buildFactorSubgraph( - const GaussianFactorGraph &gfg, const Subgraph &subgraph, - const bool clone) { - auto result = boost::make_shared(); - result->reserve(subgraph.size()); - for ( const SubgraphEdge &e: subgraph ) { - const size_t index = e.index(); - if ( clone ) result->push_back(gfg[index]->clone()); - else result->push_back(gfg[index]); - } - return result; -} } // nsamespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index a9746d104..8906337a9 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -17,30 +17,16 @@ #pragma once +#include #include -#include -#include #include #include #include -#include -#include -#include -#include #include -#include #include #include -#include -#include - -namespace boost { -namespace serialization { -class access; -} /* namespace serialization */ -} /* namespace boost */ namespace gtsam { @@ -49,142 +35,11 @@ namespace gtsam { class GaussianFactorGraph; class VectorValues; - struct GTSAM_EXPORT SubgraphEdge { - size_t index_; /* edge id */ - double weight_; /* edge weight */ - SubgraphEdge() : index_(0), weight_(1.0) {} - SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {} - SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {} - inline size_t index() const { return index_; } - inline double weight() const { return weight_; } - inline bool isUnitWeight() const { return (weight_ == 1.0); } - friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge); - private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(index_); - ar & BOOST_SERIALIZATION_NVP(weight_); - } - }; - - /**************************************************************************/ - class GTSAM_EXPORT Subgraph { - public: - typedef boost::shared_ptr shared_ptr; - typedef std::vector vector_shared_ptr; - typedef std::vector Edges; - typedef std::vector EdgeIndices; - typedef Edges::iterator iterator; - typedef Edges::const_iterator const_iterator; - - protected: - Edges edges_; /* index to the factors */ - - public: - Subgraph() {} - Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} - Subgraph(const Edges &edges) : edges_(edges) {} - Subgraph(const std::vector &indices) ; - - inline const Edges& edges() const { return edges_; } - inline size_t size() const { return edges_.size(); } - EdgeIndices edgeIndices() const; - - iterator begin() { return edges_.begin(); } - const_iterator begin() const { return edges_.begin(); } - iterator end() { return edges_.end(); } - const_iterator end() const { return edges_.end(); } - - void save(const std::string &fn) const; - static shared_ptr load(const std::string &fn); - friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); - - private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(edges_); - } - }; - - /****************************************************************************/ - struct GTSAM_EXPORT SubgraphBuilderParameters { - public: - typedef boost::shared_ptr shared_ptr; - - enum Skeleton { - /* augmented tree */ - NATURALCHAIN = 0, /* natural ordering of the graph */ - BFS, /* breadth-first search tree */ - KRUSKAL, /* maximum weighted spanning tree */ - } skeleton_ ; - - enum SkeletonWeight { /* how to weigh the graph edges */ - EQUAL = 0, /* every block edge has equal weight */ - RHS_2NORM, /* use the 2-norm of the rhs */ - LHS_FNORM, /* use the frobenius norm of the lhs */ - RANDOM, /* bounded random edge weight */ - } skeletonWeight_ ; - - enum AugmentationWeight { /* how to weigh the graph edges */ - SKELETON = 0, /* use the same weights in building the skeleton */ -// STRETCH, /* stretch in the laplacian sense */ -// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */ - } augmentationWeight_ ; - - double complexity_; /* factor multiplied with n, yields number of extra edges. */ - - SubgraphBuilderParameters() - : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {} - virtual ~SubgraphBuilderParameters() {} - - /* for serialization */ - void print() const ; - virtual void print(std::ostream &os) const ; - friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); - - static Skeleton skeletonTranslator(const std::string &s); - static std::string skeletonTranslator(Skeleton s); - static SkeletonWeight skeletonWeightTranslator(const std::string &s); - static std::string skeletonWeightTranslator(SkeletonWeight w); - static AugmentationWeight augmentationWeightTranslator(const std::string &s); - static std::string augmentationWeightTranslator(AugmentationWeight w); - }; - - /*****************************************************************************/ - class GTSAM_EXPORT SubgraphBuilder { - - public: - typedef SubgraphBuilder Base; - typedef boost::shared_ptr shared_ptr; - typedef std::vector Weights; - - SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) - : parameters_(p) {} - virtual ~SubgraphBuilder() {} - virtual boost::shared_ptr operator() (const GaussianFactorGraph &jfg) const ; - - private: - std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const ; - std::vector unary(const GaussianFactorGraph &gfg) const ; - std::vector natural_chain(const GaussianFactorGraph &gfg) const ; - std::vector bfs(const GaussianFactorGraph &gfg) const ; - std::vector kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const ; - std::vector sample(const std::vector &weights, const size_t t) const ; - Weights weights(const GaussianFactorGraph &gfg) const; - SubgraphBuilderParameters parameters_; - - }; - - /*******************************************************************************************/ struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { - typedef PreconditionerParameters Base; typedef boost::shared_ptr shared_ptr; SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) - : Base(), builderParams_(p) {} - virtual ~SubgraphPreconditionerParameters() {} - SubgraphBuilderParameters builderParams_; + : builderParams(p) {} + SubgraphBuilderParameters builderParams; }; /** @@ -300,38 +155,4 @@ namespace gtsam { /*****************************************************************************/ }; - /* get subvectors */ - Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys); - - /* set subvectors */ - void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst); - - - /* build a factor subgraph, which is defined as a set of weighted edges (factors) */ - boost::shared_ptr - buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); - - - /* sort the container and return permutation index with default comparator */ - template - std::vector sort_idx(const Container &src) - { - typedef typename Container::value_type T; - const size_t n = src.size() ; - std::vector > tmp; - tmp.reserve(n); - for ( size_t i = 0 ; i < n ; i++ ) - tmp.push_back(std::make_pair(i, src[i])); - - /* sort */ - std::stable_sort(tmp.begin(), tmp.end()) ; - - /* copy back */ - std::vector idx; idx.reserve(n); - for ( size_t i = 0 ; i < n ; i++ ) { - idx.push_back(tmp[i].first) ; - } - return idx; - } - } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index edf39e462..56b843e8d 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -18,13 +18,12 @@ */ #include + +#include #include #include #include #include -#include - -#include using namespace std; @@ -36,7 +35,7 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab, const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters) { GaussianFactorGraph::shared_ptr Ab1,Ab2; - boost::tie(Ab1, Ab2) = splitGraph(Ab); + std::tie(Ab1, Ab2) = splitGraph(Ab); if (parameters_.verbosity()) cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; @@ -91,49 +90,20 @@ VectorValues SubgraphSolver::optimize() const { } VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, + const KeyInfo &keyInfo, const 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 // +pair // SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) { - // Create disjoint set forest data structure for Kruskal algorithm - DSFMap dsf; + /* identify the subgraph structure */ + const SubgraphBuilder builder(parameters_.builderParams); + auto subgraph = builder(factorGraph); - // 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); + /* build factor subgraph */ + return splitFactorGraph(factorGraph, subgraph); } /****************************************************************************/ diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 76fe31d32..5ab1a8520 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -20,6 +20,10 @@ #pragma once #include +#include + +#include +#include // pair namespace gtsam { @@ -28,13 +32,15 @@ class GaussianFactorGraph; class GaussianBayesNet; class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters +struct GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { - public: - typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : Base() {} + SubgraphBuilderParameters builderParams; + explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : builderParams(p) {} void print() const { Base::print(); } - virtual void print(std::ostream &os) const { Base::print(os); } + virtual void print(std::ostream &os) const { + Base::print(os); + } }; /** @@ -115,10 +121,19 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { VectorValues optimize() const; /// Interface that IterativeSolver subclasses have to implement - virtual VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial) override; + VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial) override; + + /// @} + /// @name Implement interface + /// @{ + + /// Split graph using Kruskal algorithm, treating binary factors as edges. + std::pair < boost::shared_ptr, + boost::shared_ptr > splitGraph( + const GaussianFactorGraph &gfg); /// @} @@ -127,7 +142,7 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { /// @{ SubgraphSolver(const boost::shared_ptr &A, const Parameters ¶meters, const Ordering &ordering) - : SubgraphSolver(*A, parameters, ordering){}; + : SubgraphSolver(*A, parameters, ordering) {} SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &, const Parameters &, const Ordering &); SubgraphSolver(const boost::shared_ptr &Ab1, @@ -138,12 +153,6 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { const GaussianFactorGraph &, const Parameters &); /// @} #endif - - protected: - /// Split graph using Kruskal algorithm, treating binary factors as edges. - static boost::tuple, - boost::shared_ptr> - splitGraph(const GaussianFactorGraph &gfg); }; } // namespace gtsam diff --git a/tests/smallExample.h b/tests/smallExample.h index 8cd219bff..553650be8 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -26,7 +26,6 @@ #include #include #include -#include #include namespace gtsam { @@ -131,7 +130,7 @@ namespace example { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ -// inline boost::tuple planarGraph(size_t N); +// inline std::pair planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree @@ -399,7 +398,7 @@ inline std::pair createNonlinearSmoother(int T) { inline GaussianFactorGraph createSmoother(int T) { NonlinearFactorGraph nlfg; Values poses; - boost::tie(nlfg, poses) = createNonlinearSmoother(T); + std::tie(nlfg, poses) = createNonlinearSmoother(T); return *nlfg.linearize(poses); } @@ -563,7 +562,7 @@ inline Symbol key(size_t x, size_t y) { } // \namespace impl /* ************************************************************************* */ -inline boost::tuple planarGraph(size_t N) { +inline std::pair planarGraph(size_t N) { using namespace impl; // create empty graph @@ -601,7 +600,7 @@ inline boost::tuple planarGraph(size_t N) { // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros); - return boost::make_tuple(*gfg, xtrue); + return std::make_pair(*gfg, xtrue); } /* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index d9b07184b..fb9f7a5a2 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -218,7 +218,7 @@ TEST(SubgraphSolver, Solves) { SubgraphPreconditioner system; // We test on three different graphs - const auto Ab1 = planarGraph(3).get<0>(); + const auto Ab1 = planarGraph(3).first; const auto Ab2 = read("toy3D"); const auto Ab3 = read("randomGrid3D"); diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index 93101131d..cca13c822 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -15,18 +15,19 @@ * @author Yong-Dian Jian **/ +#include + #include #include #include #include -#include +#include #include #include #include #include -#include #include using namespace boost::assign; @@ -53,13 +54,33 @@ TEST( SubgraphSolver, Parameters ) LONGS_EQUAL(500, kParameters.maxIterations()); } +/* ************************************************************************* */ +TEST( SubgraphSolver, splitFactorGraph ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b + + SubgraphBuilderParameters params; + params.augmentationFactor = 0.0; + SubgraphBuilder builder(params); + auto subgraph = builder(Ab); + EXPECT_LONGS_EQUAL(9, subgraph.size()); + + GaussianFactorGraph::shared_ptr Ab1, Ab2; + std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph); + EXPECT_LONGS_EQUAL(9, Ab1->size()); + EXPECT_LONGS_EQUAL(13, Ab2->size()); +} + /* ************************************************************************* */ TEST( SubgraphSolver, constructor1 ) { // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; - boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b // 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 @@ -75,11 +96,11 @@ TEST( SubgraphSolver, constructor2 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b // Get the spanning tree GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); + std::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) @@ -95,11 +116,11 @@ TEST( SubgraphSolver, constructor3 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b // 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); + std::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();