diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 39f5a65cd..80adfc098 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -78,16 +78,19 @@ 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 w; w.reserve(m); + vector cdf; cdf.reserve(m); for ( size_t i = 0 ; i < m ; ++i ) { - w.push_back(weight[i]/sum); + cdf.push_back(weight[i]/sum); } vector acc(m); - std::partial_sum(w.begin(),w.end(),acc.begin()); + std::partial_sum(cdf.begin(),cdf.end(),acc.begin()); /* iid sample n times */ vector result; result.reserve(n); @@ -95,18 +98,18 @@ static vector iidSampler(const vector &weight, const size_t n) { for ( size_t i = 0 ; i < n ; ++i ) { const double value = rand() / denominator; /* binary search the interval containing "value" */ - vector::iterator it = std::lower_bound(acc.begin(), acc.end(), value); - size_t idx = it - acc.begin(); + const auto it = std::lower_bound(acc.begin(), acc.end(), value); + const size_t idx = it - acc.begin(); result.push_back(idx); } return result; } /*****************************************************************************/ -vector uniqueSampler(const vector &weight, const size_t n) { +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"); + if ( n > m ) throw std::invalid_argument("UniqueSampler: invalid input size"); vector result; @@ -221,11 +224,11 @@ SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslato } /****************************************************************/ -std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) { - if ( w == NATURALCHAIN )return "NATURALCHAIN"; - else if ( w == BFS ) return "BFS"; - else if ( w == KRUSKAL )return "KRUSKAL"; - else return "UNKNOWN"; +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"; } /****************************************************************/ @@ -271,7 +274,7 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(Augmentation /****************************************************************/ vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, - const vector &w) const { + const vector &weights) const { const SubgraphBuilderParameters &p = parameters_; switch (p.skeleton_) { case SubgraphBuilderParameters::NATURALCHAIN: @@ -281,7 +284,7 @@ vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, return bfs(gfg); break; case SubgraphBuilderParameters::KRUSKAL: - return kruskal(gfg, ordering, w); + return kruskal(gfg, ordering, weights); break; default: std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; @@ -357,10 +360,10 @@ vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { /****************************************************************/ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, - const vector &w) const { + const vector &weights) const { const VariableIndex variableIndex(gfg); const size_t n = variableIndex.size(); - const vector idx = sort_idx(w) ; + const vector idx = sort_idx(weights) ; /* initialize buffer */ vector result; @@ -379,7 +382,7 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, if ( dsf.find(u) != dsf.find(v) ) { dsf.merge(u, v) ; result.push_back(id) ; - sum += w[id] ; + sum += weights[id] ; if ( ++count == n-1 ) break ; } } @@ -388,14 +391,14 @@ vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, /****************************************************************/ vector SubgraphBuilder::sample(const vector &weights, const size_t t) const { - return uniqueSampler(weights, t); + return UniqueSampler(weights, t); } /****************************************************************/ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { - const SubgraphBuilderParameters &p = parameters_; - const Ordering inverse_ordering = Ordering::Natural(gfg); + 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(); @@ -411,7 +414,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg // Build spanning tree. const vector tree = buildTree(gfg, forward_ordering, weights); if ( tree.size() != n-1 ) { - throw std::runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); + throw std::runtime_error("SubgraphBuilder::operator() failure: tree.size() != n-1"); } // Downweight the tree edges to zero. diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 7995afedc..6bba3dd1c 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -129,11 +129,11 @@ namespace gtsam { 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 */ +// STRETCH, /* stretch in the laplacian sense */ +// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */ } augmentationWeight_ ; - double complexity_; + double complexity_; /* factor multiplied with n, yields number of extra edges. */ SubgraphBuilderParameters() : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {} @@ -145,7 +145,7 @@ namespace gtsam { friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); static Skeleton skeletonTranslator(const std::string &s); - static std::string skeletonTranslator(Skeleton w); + 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); @@ -170,7 +170,7 @@ namespace gtsam { 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 &w) 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_;