Merged in jlblancoc/gtsam/index-type-for-factors (pull request #409)

Type for Factor indices, dual to "Key"
release/4.3a0
José Luis Blanco-Claraco 2019-04-08 23:07:02 +00:00 committed by Frank Dellaert
commit 99aa09ce54
10 changed files with 66 additions and 32 deletions

32
gtsam.h
View File

@ -209,6 +209,38 @@ class KeyGroupMap {
bool insert2(size_t key, int val); bool insert2(size_t key, int val);
}; };
// Actually a FastSet<FactorIndex>
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 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<FactorIndex>
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 factorIndex) const;
};
//************************************************************************* //*************************************************************************
// base // base
//************************************************************************* //*************************************************************************

View File

@ -56,6 +56,9 @@ namespace gtsam {
/// Integer nonlinear key type /// Integer nonlinear key type
typedef std::uint64_t Key; typedef std::uint64_t Key;
/// Integer nonlinear factor index type
typedef std::uint64_t FactorIndex;
/// The index type for Eigen objects /// The index type for Eigen objects
typedef ptrdiff_t DenseIndex; typedef ptrdiff_t DenseIndex;

View File

@ -28,6 +28,9 @@
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {
/// Define collection types:
typedef FastVector<FactorIndex> FactorIndices;
typedef FastSet<FactorIndex> FactorIndexSet;
/** /**
* This is the base class for all factor types. It is templated on a KEY type, * This is the base class for all factor types. It is templated on a KEY type,

View File

@ -35,8 +35,8 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c
cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n";
for(KeyMap::value_type key_factors: index_) { for(KeyMap::value_type key_factors: index_) {
cout << "var " << keyFormatter(key_factors.first) << ":"; cout << "var " << keyFormatter(key_factors.first) << ":";
for(const size_t factor: key_factors.second) for(const auto index: key_factors.second)
cout << " " << factor; cout << " " << index;
cout << "\n"; cout << "\n";
} }
cout.flush(); cout.flush();
@ -48,8 +48,8 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
// run over variables, which will be hyper-edges. // run over variables, which will be hyper-edges.
for(KeyMap::value_type key_factors: index_) { for(KeyMap::value_type key_factors: index_) {
// every variable is a hyper-edge covering its factors // every variable is a hyper-edge covering its factors
for(const size_t factor: key_factors.second) for(const auto index: key_factors.second)
os << (factor+1) << " "; // base 1 os << (index+1) << " "; // base 1
os << "\n"; os << "\n";
} }
os << flush; os << flush;

View File

@ -17,6 +17,7 @@
#pragma once #pragma once
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
@ -43,7 +44,7 @@ class GTSAM_EXPORT VariableIndex {
public: public:
typedef boost::shared_ptr<VariableIndex> shared_ptr; typedef boost::shared_ptr<VariableIndex> shared_ptr;
typedef FastVector<size_t> Factors; typedef FactorIndices Factors;
typedef Factors::iterator Factor_iterator; typedef Factors::iterator Factor_iterator;
typedef Factors::const_iterator Factor_const_iterator; typedef Factors::const_iterator Factor_const_iterator;
@ -122,7 +123,7 @@ public:
* solving problems incrementally. * solving problems incrementally.
*/ */
template<class FG> template<class FG>
void augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices = boost::none); void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
/** /**
* Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement

View File

@ -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; static const bool debug = false;
if (debug) cout << "Getting affected factors for "; if (debug) cout << "Getting affected factors for ";
if (debug) { if (debug) {
@ -106,15 +106,14 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const {
} }
if (debug) cout << endl; if (debug) cout << endl;
NonlinearFactorGraph allAffected; FactorIndexSet indices;
KeySet indices;
for (const Key key : keys) { for (const Key key : keys) {
const VariableIndex::Factors& factors(variableIndex_[key]); const VariableIndex::Factors& factors(variableIndex_[key]);
indices.insert(factors.begin(), factors.end()); indices.insert(factors.begin(), factors.end());
} }
if (debug) cout << "Affected factors are: "; if (debug) cout << "Affected factors are: ";
if (debug) { if (debug) {
for (const size_t index : indices) { for (const auto index : indices) {
cout << index << " "; cout << index << " ";
} }
} }
@ -132,8 +131,6 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(
KeySet candidates = getAffectedFactors(affectedKeys); KeySet candidates = getAffectedFactors(affectedKeys);
gttoc(getAffectedFactors); gttoc(getAffectedFactors);
NonlinearFactorGraph nonlinearAffectedFactors;
gttic(affectedKeysSet); gttic(affectedKeysSet);
// for fast lookup below // for fast lookup below
KeySet affectedKeysSet; KeySet affectedKeysSet;
@ -589,7 +586,7 @@ ISAM2Result ISAM2::update(
// Remove the removed factors // Remove the removed factors
NonlinearFactorGraph removeFactors; NonlinearFactorGraph removeFactors;
removeFactors.reserve(removeFactorIndices.size()); removeFactors.reserve(removeFactorIndices.size());
for (size_t index : removeFactorIndices) { for (const auto index : removeFactorIndices) {
removeFactors.push_back(nonlinearFactors_[index]); removeFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(index); nonlinearFactors_.remove(index);
if (params_.cacheLinearizedFactors) linearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
@ -823,7 +820,7 @@ void ISAM2::marginalizeLeaves(
KeySet leafKeysRemoved; KeySet leafKeysRemoved;
// Keep track of factors that get summarized by removing cliques // Keep track of factors that get summarized by removing cliques
KeySet factorIndicesToRemove; FactorIndexSet factorIndicesToRemove;
// Remove the subtree and throw away the cliques // Remove the subtree and throw away the cliques
auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) { auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) {
@ -937,8 +934,8 @@ void ISAM2::marginalizeLeaves(
} }
} }
// Create factor graph from factor indices // Create factor graph from factor indices
for (size_t i : factorsFromMarginalizedInClique_step1) { for (const auto index: factorsFromMarginalizedInClique_step1) {
graph.push_back(nonlinearFactors_[i]->linearize(theta_)); graph.push_back(nonlinearFactors_[index]->linearize(theta_));
} }
// Reeliminate the linear graph to get the marginal and discard the // Reeliminate the linear graph to get the marginal and discard the
@ -1011,10 +1008,10 @@ void ISAM2::marginalizeLeaves(
// Remove the factors to remove that have been summarized in the newly-added // Remove the factors to remove that have been summarized in the newly-added
// marginal factors // marginal factors
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
for (size_t i : factorIndicesToRemove) { for (const auto index: factorIndicesToRemove) {
removedFactors.push_back(nonlinearFactors_[i]); removedFactors.push_back(nonlinearFactors_[index]);
nonlinearFactors_.remove(i); nonlinearFactors_.remove(index);
if (params_.cacheLinearizedFactors) linearFactors_.remove(i); if (params_.cacheLinearizedFactors) linearFactors_.remove(index);
} }
variableIndex_.remove(factorIndicesToRemove.begin(), variableIndex_.remove(factorIndicesToRemove.begin(),
factorIndicesToRemove.end(), removedFactors); factorIndicesToRemove.end(), removedFactors);

View File

@ -292,7 +292,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
*/ */
void expmapMasked(const KeySet& mask); void expmapMasked(const KeySet& mask);
FastSet<Key> getAffectedFactors(const FastList<Key>& keys) const; FactorIndexSet getAffectedFactors(const FastList<Key>& keys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(
const FastList<Key>& affectedKeys, const KeySet& relinKeys) const; const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans);

View File

@ -31,8 +31,6 @@
namespace gtsam { namespace gtsam {
typedef FastVector<size_t> FactorIndices;
/** /**
* @addtogroup ISAM2 * @addtogroup ISAM2
* This struct is returned from ISAM2::update() and contains information about * This struct is returned from ISAM2::update() and contains information about

View File

@ -175,19 +175,19 @@ private:
// collect all factors involving this key in the original graph // collect all factors involving this key in the original graph
DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph());
for(size_t factorIdx: varIndex[key]) { for(size_t factorIndex: varIndex[key]) {
star->push_back(graph.at(factorIdx)); star->push_back(graph.at(factorIndex));
// accumulate unary factors // accumulate unary factors
if (graph.at(factorIdx)->size() == 1) { if (graph.at(factorIndex)->size() == 1) {
if (!prodOfUnaries) if (!prodOfUnaries)
prodOfUnaries = boost::dynamic_pointer_cast<DecisionTreeFactor>( prodOfUnaries = boost::dynamic_pointer_cast<DecisionTreeFactor>(
graph.at(factorIdx)); graph.at(factorIndex));
else else
prodOfUnaries = boost::make_shared<DecisionTreeFactor>( prodOfUnaries = boost::make_shared<DecisionTreeFactor>(
*prodOfUnaries *prodOfUnaries
* (*boost::dynamic_pointer_cast<DecisionTreeFactor>( * (*boost::dynamic_pointer_cast<DecisionTreeFactor>(
graph.at(factorIdx)))); graph.at(factorIndex))));
} }
} }