From 285ebd7dbdfbe2d3793124428587257866953576 Mon Sep 17 00:00:00 2001 From: jlblancoc Date: Wed, 27 Feb 2019 08:50:50 +0100 Subject: [PATCH 1/2] Type for Factor indices, dual to "Key" This avoids a couple of confusing uses of KeySet to refer to lists of Factors, and makes code more readable where formerly using size_t to index factors. --- gtsam.h | 38 +++++++++++++++++-- gtsam/base/types.h | 3 ++ gtsam/inference/Factor.h | 3 ++ gtsam/inference/VariableIndex.cpp | 4 +- gtsam/inference/VariableIndex.h | 5 ++- gtsam/nonlinear/ISAM2.cpp | 17 ++++----- gtsam/nonlinear/ISAM2.h | 2 +- gtsam/nonlinear/ISAM2Result.h | 2 - .../nonlinear/ConcurrentIncrementalFilter.cpp | 2 +- 9 files changed, 55 insertions(+), 21 deletions(-) diff --git a/gtsam.h b/gtsam.h index ba3b7f7c7..c5cea7549 100644 --- a/gtsam.h +++ b/gtsam.h @@ -209,6 +209,38 @@ class KeyGroupMap { bool insert2(size_t key, int val); }; +// Actually a FastSet +class FactorIndexSet { + FactorIndexSet(); + FactorIndexSet(const gtsam::FactorIndexSet& set); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t factorIdx); + bool erase(size_t factorIdx); // returns true if value was removed + bool count(size_t factorIdx) const; // returns true if value exists +}; + +// Actually a vector +class FactorIndices { + FactorIndices(); + FactorIndices(const gtsam::FactorIndices& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t factorIdx) const; +}; //************************************************************************* // base //************************************************************************* @@ -1219,7 +1251,7 @@ namespace noiseModel { #include virtual class Base { void print(string s) const; - // Methods below are available for all noise models. However, can't add them + // Methods below are available for all noise models. However, can't add them // because wrap (incorrectly) thinks robust classes derive from this Base as well. // bool isConstrained() const; // bool isUnit() const; @@ -1257,7 +1289,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { Vector sigmas() const; Vector invsigmas() const; Vector precisions() const; - + // enabling serialization functionality void serializable() const; }; @@ -2053,7 +2085,7 @@ virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { bool getUseFixedLambdaFactor(); string getLogFile() const; string getVerbosityLM() const; - + void setDiagonalDamping(bool flag); void setlambdaFactor(double value); void setlambdaInitial(double value); diff --git a/gtsam/base/types.h b/gtsam/base/types.h index c4775b672..3b6c9f337 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -56,6 +56,9 @@ namespace gtsam { /// Integer nonlinear key type typedef std::uint64_t Key; + /// Integer nonlinear factor index type + typedef std::uint64_t FactorIndex; + /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index d44d82954..40326a4cc 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -28,6 +28,9 @@ #include namespace gtsam { +/// Define collection types: +typedef FastVector FactorIndices; +typedef FastSet FactorIndexSet; /** * This is the base class for all factor types. It is templated on a KEY type, diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 72c56be5b..6d7471e31 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -35,7 +35,7 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; for(KeyMap::value_type key_factors: index_) { cout << "var " << keyFormatter(key_factors.first) << ":"; - for(const size_t factor: key_factors.second) + for(const FactorIndex factor: key_factors.second) cout << " " << factor; cout << "\n"; } @@ -48,7 +48,7 @@ void VariableIndex::outputMetisFormat(ostream& os) const { // run over variables, which will be hyper-edges. for(KeyMap::value_type key_factors: index_) { // every variable is a hyper-edge covering its factors - for(const size_t factor: key_factors.second) + for(const FactorIndex factor: key_factors.second) os << (factor+1) << " "; // base 1 os << "\n"; } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index c16946f80..5f9e05cb0 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,6 +17,7 @@ #pragma once +#include #include #include #include @@ -43,7 +44,7 @@ class GTSAM_EXPORT VariableIndex { public: typedef boost::shared_ptr shared_ptr; - typedef FastVector Factors; + typedef FactorIndices Factors; typedef Factors::iterator Factor_iterator; typedef Factors::const_iterator Factor_const_iterator; @@ -122,7 +123,7 @@ public: * solving problems incrementally. */ template - void augment(const FG& factors, boost::optional&> newFactorIndices = boost::none); + void augment(const FG& factors, boost::optional newFactorIndices = boost::none); /** * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 32a24b51d..4b4edba5f 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -96,7 +96,7 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { +FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; if (debug) cout << "Getting affected factors for "; if (debug) { @@ -106,15 +106,14 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { } if (debug) cout << endl; - NonlinearFactorGraph allAffected; - KeySet indices; + FactorIndexSet indices; for (const Key key : keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } if (debug) cout << "Affected factors are: "; if (debug) { - for (const size_t index : indices) { + for (const FactorIndex index : indices) { cout << index << " "; } } @@ -132,8 +131,6 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); - NonlinearFactorGraph nonlinearAffectedFactors; - gttic(affectedKeysSet); // for fast lookup below KeySet affectedKeysSet; @@ -589,7 +586,7 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for (size_t index : removeFactorIndices) { + for (FactorIndex index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -823,7 +820,7 @@ void ISAM2::marginalizeLeaves( KeySet leafKeysRemoved; // Keep track of factors that get summarized by removing cliques - KeySet factorIndicesToRemove; + FactorIndexSet factorIndicesToRemove; // Remove the subtree and throw away the cliques auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) { @@ -937,7 +934,7 @@ void ISAM2::marginalizeLeaves( } } // Create factor graph from factor indices - for (size_t i : factorsFromMarginalizedInClique_step1) { + for (FactorIndex i : factorsFromMarginalizedInClique_step1) { graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } @@ -1011,7 +1008,7 @@ void ISAM2::marginalizeLeaves( // Remove the factors to remove that have been summarized in the newly-added // marginal factors NonlinearFactorGraph removedFactors; - for (size_t i : factorIndicesToRemove) { + for (FactorIndex i : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); if (params_.cacheLinearizedFactors) linearFactors_.remove(i); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 04bd3d3eb..86b878837 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -292,7 +292,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { */ void expmapMasked(const KeySet& mask); - FastSet getAffectedFactors(const FastList& keys) const; + FactorIndexSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 1539d90c4..763a5e198 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -31,8 +31,6 @@ namespace gtsam { -typedef FastVector FactorIndices; - /** * @addtogroup ISAM2 * This struct is returned from ISAM2::update() and contains information about diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index e7c8229ef..78bd977f5 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) From 4fb718a943e5565d0e5e1e8dfc3e186e96e61920 Mon Sep 17 00:00:00 2001 From: jlblancoc Date: Tue, 9 Apr 2019 00:29:31 +0200 Subject: [PATCH 2/2] prefer auto in range for loops --- gtsam.h | 8 ++++---- gtsam/inference/VariableIndex.cpp | 8 ++++---- gtsam/nonlinear/ISAM2.cpp | 16 ++++++++-------- .../discrete/tests/testLoopyBelief.cpp | 10 +++++----- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam.h b/gtsam.h index c5cea7549..8e6d308e1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -220,9 +220,9 @@ class FactorIndexSet { void clear(); // structure specific methods - void insert(size_t factorIdx); - bool erase(size_t factorIdx); // returns true if value was removed - bool count(size_t factorIdx) const; // returns true if value exists + void insert(size_t factorIndex); + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists }; // Actually a vector @@ -239,7 +239,7 @@ class FactorIndices { size_t at(size_t i) const; size_t front() const; size_t back() const; - void push_back(size_t factorIdx) const; + void push_back(size_t factorIndex) const; }; //************************************************************************* // base diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 6d7471e31..b71c81988 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -35,8 +35,8 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; for(KeyMap::value_type key_factors: index_) { cout << "var " << keyFormatter(key_factors.first) << ":"; - for(const FactorIndex factor: key_factors.second) - cout << " " << factor; + for(const auto index: key_factors.second) + cout << " " << index; cout << "\n"; } cout.flush(); @@ -48,8 +48,8 @@ void VariableIndex::outputMetisFormat(ostream& os) const { // run over variables, which will be hyper-edges. for(KeyMap::value_type key_factors: index_) { // every variable is a hyper-edge covering its factors - for(const FactorIndex factor: key_factors.second) - os << (factor+1) << " "; // base 1 + for(const auto index: key_factors.second) + os << (index+1) << " "; // base 1 os << "\n"; } os << flush; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 4b4edba5f..c0b2d0757 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -113,7 +113,7 @@ FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { } if (debug) cout << "Affected factors are: "; if (debug) { - for (const FactorIndex index : indices) { + for (const auto index : indices) { cout << index << " "; } } @@ -586,7 +586,7 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for (FactorIndex index : removeFactorIndices) { + for (const auto index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -934,8 +934,8 @@ void ISAM2::marginalizeLeaves( } } // Create factor graph from factor indices - for (FactorIndex i : factorsFromMarginalizedInClique_step1) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_)); + for (const auto index: factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[index]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the @@ -1008,10 +1008,10 @@ void ISAM2::marginalizeLeaves( // Remove the factors to remove that have been summarized in the newly-added // marginal factors NonlinearFactorGraph removedFactors; - for (FactorIndex i : factorIndicesToRemove) { - removedFactors.push_back(nonlinearFactors_[i]); - nonlinearFactors_.remove(i); - if (params_.cacheLinearizedFactors) linearFactors_.remove(i); + for (const auto index: factorIndicesToRemove) { + removedFactors.push_back(nonlinearFactors_[index]); + nonlinearFactors_.remove(index); + if (params_.cacheLinearizedFactors) linearFactors_.remove(index); } variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index d2efc3a2d..262c575c1 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -175,19 +175,19 @@ private: // collect all factors involving this key in the original graph DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); - for(size_t factorIdx: varIndex[key]) { - star->push_back(graph.at(factorIdx)); + for(size_t factorIndex: varIndex[key]) { + star->push_back(graph.at(factorIndex)); // accumulate unary factors - if (graph.at(factorIdx)->size() == 1) { + if (graph.at(factorIndex)->size() == 1) { if (!prodOfUnaries) prodOfUnaries = boost::dynamic_pointer_cast( - graph.at(factorIdx)); + graph.at(factorIndex)); else prodOfUnaries = boost::make_shared( *prodOfUnaries * (*boost::dynamic_pointer_cast( - graph.at(factorIdx)))); + graph.at(factorIndex)))); } }