From 6b637bda9e1c758d7ac3497707dbd408c198481d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Apr 2019 23:01:32 -0400 Subject: [PATCH] Cleanup --- gtsam/linear/SubgraphPreconditioner.cpp | 158 +++++++++++++----------- 1 file changed, 87 insertions(+), 71 deletions(-) diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index de0df8192..c75bcd4e2 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -52,17 +52,21 @@ #include #include -using namespace std; +using std::cout; +using std::endl; +using std::vector; +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) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - for(const GaussianFactor::shared_ptr &gf: gfg) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); + auto result = boost::make_shared(); + for (const auto &factor : gfg) { + auto jf = boost::dynamic_pointer_cast(factor); if( !jf ) { - jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + jf = boost::make_shared(*factor); } result->push_back(jf); } @@ -70,7 +74,7 @@ static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFa } /*****************************************************************************/ -static std::vector iidSampler(const vector &weight, const size_t n) { +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); @@ -107,10 +111,10 @@ vector uniqueSampler(const vector &weight, const size_t n) { vector result; size_t count = 0; - std::vector touched(m, false); + vector touched(m, false); while ( count < n ) { - std::vector localIndices; localIndices.reserve(n-count); - std::vector localWeights; localWeights.reserve(n-count); + vector localIndices; localIndices.reserve(n-count); + vector localWeights; localWeights.reserve(n-count); /* collect data */ for ( size_t i = 0 ; i < m ; ++i ) { @@ -134,16 +138,16 @@ vector uniqueSampler(const vector &weight, const size_t n) { } /****************************************************************************/ -Subgraph::Subgraph(const std::vector &indices) { +Subgraph::Subgraph(const vector &indices) { edges_.reserve(indices.size()); for ( const size_t &idx: indices ) { - edges_.push_back(SubgraphEdge(idx, 1.0)); + edges_.emplace_back(idx, 1.0); } } /****************************************************************************/ -std::vector Subgraph::edgeIndices() const { - std::vector eid; eid.reserve(size()); +vector Subgraph::edgeIndices() const { + vector eid; eid.reserve(size()); for ( const SubgraphEdge &edge: edges_ ) { eid.push_back(edge.index_); } @@ -169,7 +173,7 @@ Subgraph::shared_ptr Subgraph::load(const std::string &fn) { } /****************************************************************************/ -std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { +ostream &operator<<(ostream &os, const SubgraphEdge &edge) { if ( edge.weight() != 1.0 ) os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; else @@ -178,7 +182,7 @@ std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { } /****************************************************************************/ -std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { +ostream &operator<<(ostream &os, const Subgraph &subgraph) { os << "Subgraph" << endl; for ( const SubgraphEdge &e: subgraph.edges() ) { os << e << ", " ; @@ -212,7 +216,7 @@ SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslato if (s == "NATURALCHAIN") return NATURALCHAIN; else if (s == "BFS") return BFS; else if (s == "KRUSKAL") return KRUSKAL; - throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); + throw std::invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); return KRUSKAL; } @@ -231,7 +235,7 @@ SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWei else if (s == "RHS") return RHS_2NORM; else if (s == "LHS") return LHS_FNORM; else if (s == "RANDOM") return RANDOM; - throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); + throw std::invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); return EQUAL; } @@ -245,12 +249,14 @@ std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w } /****************************************************************/ -SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) { +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 invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); + throw std::invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); return SKELETON; } @@ -263,7 +269,9 @@ std::string SubgraphBuilderParameters::augmentationWeightTranslator(Augmentation } /****************************************************************/ -std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { +vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &w) const { const SubgraphBuilderParameters &p = parameters_; switch (p.skeleton_) { case SubgraphBuilderParameters::NATURALCHAIN: @@ -276,18 +284,18 @@ std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, c return kruskal(gfg, ordering, w); break; default: - cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; + std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; break; } return vector(); } /****************************************************************/ -std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { - std::vector result ; +vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { + vector result ; size_t idx = 0; - for ( const GaussianFactor::shared_ptr &gf: gfg ) { - if ( gf->size() == 1 ) { + for (const auto &factor : gfg) { + if ( factor->size() == 1 ) { result.push_back(idx); } idx++; @@ -296,8 +304,8 @@ std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const } /****************************************************************/ -std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { - std::vector result ; +vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { + vector result ; size_t idx = 0; for ( const GaussianFactor::shared_ptr &gf: gfg ) { if ( gf->size() == 2 ) { @@ -311,7 +319,7 @@ std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gf } /****************************************************************/ -std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { +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]; @@ -319,7 +327,7 @@ std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { const size_t n = variableIndex.size(); /* each vertex has self as the predecessor */ - std::vector result; + vector result; result.reserve(n-1); /* Initialize */ @@ -347,7 +355,9 @@ std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { } /****************************************************************/ -std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { +vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &w) const { const VariableIndex variableIndex(gfg); const size_t n = variableIndex.size(); const vector idx = sort_idx(w) ; @@ -357,18 +367,17 @@ std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con result.reserve(n-1); // container for acsendingly sorted edges - DSFVector D(n) ; + DSFVector dsf(n); size_t count = 0 ; double sum = 0.0 ; for (const size_t id: idx) { const GaussianFactor &gf = *gfg[id]; - if ( gf.keys().size() != 2 ) continue; - const size_t u = ordering.find(gf.keys()[0])->second, - u_root = D.find(u), - v = ordering.find(gf.keys()[1])->second, - v_root = D.find(v) ; - if ( u_root != v_root ) { - D.merge(u_root, v_root) ; + 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(id) ; sum += w[id] ; if ( ++count == n-1 ) break ; @@ -378,7 +387,7 @@ std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, con } /****************************************************************/ -std::vector SubgraphBuilder::sample(const std::vector &weights, const size_t t) const { +vector SubgraphBuilder::sample(const vector &weights, const size_t t) const { return uniqueSampler(weights, t); } @@ -395,7 +404,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg /* sanity check */ if ( tree.size() != n-1 ) { - throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); + throw std::runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); } /* down weight the tree edges to zero */ @@ -404,7 +413,7 @@ Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg } /* decide how many edges to augment */ - std::vector offTree = sample(w, t); + vector offTree = sample(w, t); vector subgraph = unary(gfg); subgraph.insert(subgraph.end(), tree.begin(), tree.end()); @@ -450,7 +459,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg break; default: - throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); + throw std::invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); break; } } @@ -484,21 +493,20 @@ double SubgraphPreconditioner::error(const VectorValues& y) const { /* ************************************************************************* */ // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), -VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { +VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const { VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ - Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ + Errors e = (*Ab2() * x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ VectorValues v = VectorValues::Zero(x); - Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ + Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ return y + Rc1()->backSubstituteTranspose(v); } /* ************************************************************************* */ // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { - Errors e(y); VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ - Errors e2 = *Ab2() * x; /* A2*x */ + Errors e2 = *Ab2() * x; /* A2*x */ e.splice(e.end(), e2); return e; } @@ -568,47 +576,55 @@ void SubgraphPreconditioner::print(const std::string& s) const { } /*****************************************************************************/ -void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const -{ +void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const { /* copy first */ + assert(x.size() == y.size()); std::copy(y.data(), y.data() + y.rows(), x.data()); /* in place back substitute */ - for (auto cg: boost::adaptors::reverse(*Rc1_)) { + for (const auto &cg : boost::adaptors::reverse(*Rc1_)) { /* collect a subvector of x that consists of the parents of cg (S) */ - const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents())); - const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); + const Vector xParent = getSubvector( + x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents())); + const Vector rhsFrontal = getSubvector( + x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); /* compute the solution for the current pivot */ - const Vector solFrontal = cg->get_R().triangularView().solve(rhsFrontal - cg->get_S() * xParent); + const Vector solFrontal = cg->get_R().triangularView().solve( + rhsFrontal - cg->get_S() * xParent); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x); + setSubvector(solFrontal, keyInfo_, + KeyVector(cg->beginFrontals(), cg->endFrontals()), x); } } /*****************************************************************************/ -void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const -{ +void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const { /* copy first */ + assert(x.size() == y.size()); std::copy(y.data(), y.data() + y.rows(), x.data()); /* in place back substitute */ - for(const boost::shared_ptr & cg: *Rc1_) { - const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); -// const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); - const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); + for (const auto &cg : *Rc1_) { + const Vector rhsFrontal = getSubvector( + x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); + const Vector solFrontal = + cg->get_R().transpose().triangularView().solve( + rhsFrontal); // Check for indeterminant solution - if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front()); + if (solFrontal.hasNaN()) + throw IndeterminantLinearSystemException(cg->keys().front()); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x); + setSubvector(solFrontal, keyInfo_, + KeyVector(cg->beginFrontals(), cg->endFrontals()), x); /* substract from parent variables */ - for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { - KeyInfo::const_iterator it2 = keyInfo_.find(*it); - Eigen::Map rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1); + for (auto it = cg->beginParents(); it != cg->endParents(); it++) { + const KeyInfoEntry &info = keyInfo_.find(*it)->second; + Eigen::Map rhsParent(x.data() + info.colstart(), info.dim(), 1); rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; } } @@ -634,14 +650,14 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) { /* a cache of starting index and dim */ - typedef vector > Cache; + typedef vector > Cache; Cache cache; /* figure out dimension by traversing the keys */ size_t d = 0; for ( const Key &key: keys ) { const KeyInfoEntry &entry = keyInfo.find(key)->second; - cache.push_back(make_pair(entry.colstart(), entry.dim())); + cache.emplace_back(entry.colstart(), entry.dim()); d += entry.dim(); } @@ -668,10 +684,10 @@ void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &ke } /*****************************************************************************/ -boost::shared_ptr -buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) { - - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); +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 idx = e.index();