12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added comment groupings to inference

release/4.3a0
Nick Barrash 2012-01-23 19:54:45 +00:00
parent a8607d284d
commit f7865c80b1
18 changed files with 626 additions and 277 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
gtsam/inference/BayesNet.h
View File

@ -38,6 +38,7 @@ namespace gtsam {
*
* todo: Symbolic using Index is a misnomer.
* todo: how to handle Bayes nets with an optimize function? Currently using global functions.
* \nosubgrouping
*/
template<class CONDITIONAL>
class BayesNet {
@ -67,18 +68,52 @@ protected:
public:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */
BayesNet() {};
/** BayesNet with 1 conditional */
BayesNet(const sharedConditional& conditional) { push_back(conditional); }
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string& s = "") const;
/** check equality */
bool equals(const BayesNet& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** size is the number of nodes */
size_t size() const {
return conditionals_.size();
}
/** return keys in reverse topological sort order, i.e., elimination order */
FastList<Index> ordering() const;
/** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](Index key) const;
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
/** return iterators. FD: breaks encapsulation? */
const_iterator begin() const {return conditionals_.begin();} ///<TODO: comment
const_iterator end() const {return conditionals_.end();} ///<TODO: comment
const_reverse_iterator rbegin() const {return conditionals_.rbegin();} ///<TODO: comment
const_reverse_iterator rend() const {return conditionals_.rend();} ///<TODO: comment
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
@ -87,6 +122,43 @@ public:
*/
const_iterator find(Index key) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Remove any leaf conditional. The conditional to remove is specified by
* iterator. To find the iterator pointing to the conditional containing a
* particular key, use find(), which has \f$ O(n) \f$ complexity. The
* popLeaf function by itself has \f$ O(1) \f$ complexity.
*
* If gtsam is compiled without NDEBUG defined, this function will check that
* the node is indeed a leaf, but otherwise will not check, because the check
* has \f$ O(n^2) \f$ complexity.
*
* Example 1:
\code
// Remove a leaf node with a known conditional
GaussianBayesNet gbn = ...
GaussianBayesNet::iterator leafConditional = ...
gbn.popLeaf(leafConditional);
\endcode
* Example 2:
\code
// Remove the leaf node containing variable index 14
GaussianBayesNet gbn = ...
gbn.popLeaf(gbn.find(14));
\endcode
* @param conditional The iterator pointing to the leaf conditional to remove
*/
void popLeaf(iterator conditional);
/** return last node in ordering */
sharedConditional& front() { return conditionals_.front(); }
/** return last node in ordering */
sharedConditional& back() { return conditionals_.back(); }
/** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this
* key. Running time is approximately \f$ O(n) \f$ in the number of
@ -124,33 +196,6 @@ public:
*/
void pop_front() {conditionals_.pop_front();}
/**
* Remove any leaf conditional. The conditional to remove is specified by
* iterator. To find the iterator pointing to the conditional containing a
* particular key, use find(), which has \f$ O(n) \f$ complexity. The
* popLeaf function by itself has \f$ O(1) \f$ complexity.
*
* If gtsam is compiled without NDEBUG defined, this function will check that
* the node is indeed a leaf, but otherwise will not check, because the check
* has \f$ O(n^2) \f$ complexity.
*
* Example 1:
\code
// Remove a leaf node with a known conditional
GaussianBayesNet gbn = ...
GaussianBayesNet::iterator leafConditional = ...
gbn.popLeaf(leafConditional);
\endcode
* Example 2:
\code
// Remove the leaf node containing variable index 14
GaussianBayesNet gbn = ...
gbn.popLeaf(gbn.find(14));
\endcode
* @param conditional The iterator pointing to the leaf conditional to remove
*/
void popLeaf(iterator conditional);
/** Permute the variables in the BayesNet */
void permuteWithInverse(const Permutation& inversePermutation);
@ -161,38 +206,10 @@ public:
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** size is the number of nodes */
size_t size() const {
return conditionals_.size();
}
/** return keys in reverse topological sort order, i.e., elimination order */
FastList<Index> ordering() const;
/** SLOW O(n) random access to Conditional by key */
sharedConditional operator[](Index key) const;
/** return last node in ordering */
sharedConditional& front() { return conditionals_.front(); }
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> front() const { return conditionals_.front(); }
/** return last node in ordering */
sharedConditional& back() { return conditionals_.back(); }
/** return last node in ordering */
boost::shared_ptr<const CONDITIONAL> back() const { return conditionals_.back(); }
/** return iterators. FD: breaks encapsulation? */
const_iterator begin() const {return conditionals_.begin();}
const_iterator end() const {return conditionals_.end();}
const_reverse_iterator rbegin() const {return conditionals_.rbegin();}
const_reverse_iterator rend() const {return conditionals_.rend();}
iterator begin() {return conditionals_.begin();}
iterator end() {return conditionals_.end();}
reverse_iterator rbegin() {return conditionals_.rbegin();}
reverse_iterator rend() {return conditionals_.rend();}
iterator begin() {return conditionals_.begin();} ///<TODO: comment
iterator end() {return conditionals_.end();} ///<TODO: comment
reverse_iterator rbegin() {return conditionals_.rbegin();} ///<TODO: comment
reverse_iterator rend() {return conditionals_.rend();} ///<TODO: comment
/** saves the bayes to a text file in GraphViz format */
// void saveGraph(const std::string& s) const;
@ -204,6 +221,8 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_);
}
/// @}
}; // BayesNet
} // namespace gtsam

113
gtsam/inference/BayesTree.h
View File

@ -46,6 +46,7 @@ namespace gtsam {
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
*
* \ingroup Multifrontal
* \nosubgrouping
*/
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
class BayesTree {
@ -128,12 +129,19 @@ namespace gtsam {
public:
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */
BayesTree();
/** Create a Bayes Tree from a Bayes Net */
BayesTree(const BayesNet<CONDITIONAL>& bayesNet);
/// @}
/// @name Advanced Constructors
/// @{
/**
* Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the
* new root clique and the subtrees are connected to the root clique.
@ -143,53 +151,19 @@ namespace gtsam {
/** Destructor */
virtual ~BayesTree() {}
/**
* Constructing Bayes trees
*/
/** Insert a new conditional
* This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case.
* */
static void insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
/**
* Hang a new subtree off of the existing tree. This finds the appropriate
* parent clique for the subtree (which may be the root), and updates the
* nodes index with the new cliques in the subtree. None of the frontal
* variables in the subtree may appear in the separators of the existing
* BayesTree.
*/
void insert(const sharedClique& subtree);
/**
* Querying Bayes trees
*/
/// @}
/// @name Testable
/// @{
/** check equality */
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
void cloneTo(shared_ptr& newTree) const {
cloneTo(newTree, root(), sharedClique());
}
/** print */
void print(const std::string& s = "") const;
private:
/** deep copy from another tree */
void cloneTo(shared_ptr& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone());
newTree->addClique(newClique, parent);
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
cloneTo(newTree, childClique, newClique);
}
}
/// @}
/// @name Standard Interface
/// @{
public:
@ -214,9 +188,6 @@ namespace gtsam {
/** return root clique */
const sharedClique& root() const { return root_; }
/** Access the root clique (non-const version) */
sharedClique& root() { return root_; }
/** find the clique to which key belongs */
sharedClique operator[](Index key) const {
return nodes_.at(key);
@ -245,15 +216,15 @@ namespace gtsam {
* Read only with side effects
*/
/** print */
void print(const std::string& s = "") const;
/** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s) const;
/**
* Altering Bayes trees
*/
/// @}
/// @name Advanced Interface
/// @{
/** Access the root clique (non-const version) */
sharedClique& root() { return root_; }
/** Remove all nodes */
void clear();
@ -271,7 +242,45 @@ namespace gtsam {
template<class CONTAINER>
void removeTop(const CONTAINER& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
/**
* Hang a new subtree off of the existing tree. This finds the appropriate
* parent clique for the subtree (which may be the root), and updates the
* nodes index with the new cliques in the subtree. None of the frontal
* variables in the subtree may appear in the separators of the existing
* BayesTree.
*/
void insert(const sharedClique& subtree);
/** Insert a new conditional
* This function only applies for Symbolic case with IndexCondtional,
* We make it static so that it won't be compiled in GaussianConditional case.
* */
static void insert(BayesTree<CONDITIONAL,CLIQUE>& bayesTree, const sharedConditional& conditional);
/**
* Insert a new clique corresponding to the given Bayes net.
* It is the caller's responsibility to decide whether the given Bayes net is a valid clique,
* i.e. all the variables (frontal and separator) are connected
*/
sharedClique insert(const sharedConditional& clique,
std::list<sharedClique>& children, bool isRootClique = false);
///TODO: comment
void cloneTo(shared_ptr& newTree) const {
cloneTo(newTree, root(), sharedClique());
}
private:
/** deep copy from another tree */
void cloneTo(shared_ptr& newTree, const sharedClique& subtree, const sharedClique& parent) const {
sharedClique newClique(subtree->clone());
newTree->addClique(newClique, parent);
BOOST_FOREACH(const sharedClique& childClique, subtree->children()) {
cloneTo(newTree, childClique, newClique);
}
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
@ -280,6 +289,8 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(root_);
}
/// @}
}; // BayesTree

93
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -41,6 +41,7 @@ namespace gtsam {
*
* @tparam DERIVED The derived clique type.
* @tparam CONDITIONAL The conditional type.
* \nosubgrouping
*/
template<class DERIVED, class CONDITIONAL>
struct BayesTreeCliqueBase {
@ -58,7 +59,9 @@ namespace gtsam {
typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
protected:
void assertInvariants() const;
/// @name Standard Constructors
/// @{
/** Default constructor */
BayesTreeCliqueBase() {}
@ -69,11 +72,57 @@ namespace gtsam {
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
BayesTreeCliqueBase(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result);
/// @}
public:
sharedConditional conditional_;
derived_weak_ptr parent_;
std::list<derived_ptr> children_;
/// @name Testable
/// @{
/** check equality */
bool equals(const This& other, double tol=1e-9) const {
return (!conditional_ && !other.conditional()) ||
conditional_->equals(*other.conditional(), tol);
}
/** print this node */
void print(const std::string& s = "") const;
/** print this node and entire subtree below it */
void printTree(const std::string& indent="") const;
/// @}
/// @name Standard Interface
/// @{
/** Access the conditional */
const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_.expired(); }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** The arrow operator accesses the conditional */
const ConditionalType* operator->() const { return conditional_.get(); }
///TODO: comment
const std::list<derived_ptr>& children() const { return children_; }
/// @}
/// @name Advanced Interface
/// @{
/** The arrow operator accesses the conditional */
ConditionalType* operator->() { return conditional_.get(); }
/** return the const reference of children */
std::list<derived_ptr>& children() { return children_; }
/** Construct shared_ptr from a conditional, leaving parent and child pointers uninitialized */
static derived_ptr Create(const sharedConditional& conditional) { return boost::make_shared<DerivedType>(conditional); }
@ -82,34 +131,10 @@ namespace gtsam {
* types, such as ISAM2Clique, the factor part is kept as a cached factor.
* @param An elimination result, which is a pair<CONDITIONAL,FACTOR>
*/
static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
static derived_ptr Create(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) { return boost::make_shared<DerivedType>(result); }
derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); }
/** print this node */
void print(const std::string& s = "") const;
/** The arrow operator accesses the conditional */
const ConditionalType* operator->() const { return conditional_.get(); }
/** The arrow operator accesses the conditional */
ConditionalType* operator->() { return conditional_.get(); }
/** Access the conditional */
const sharedConditional& conditional() const { return conditional_; }
/** is this the root of a Bayes tree ? */
inline bool isRoot() const { return parent_.expired(); }
/** return the const reference of children */
std::list<derived_ptr>& children() { return children_; }
const std::list<derived_ptr>& children() const { return children_; }
/** The size of subtree rooted at this clique, i.e., nr of Cliques */
size_t treeSize() const;
/** print this node and entire subtree below it */
void printTree(const std::string& indent="") const;
///TODO: comment
derived_ptr clone() const { return Create(sharedConditional(new ConditionalType(*conditional_))); }
/** Permute the variables in the whole subtree rooted at this clique */
void permuteWithInverse(const Permutation& inversePermutation);
@ -131,13 +156,13 @@ namespace gtsam {
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function);
bool equals(const This& other, double tol=1e-9) const {
return (!conditional_ && !other.conditional()) ||
conditional_->equals(*other.conditional(), tol);
}
friend class BayesTree<ConditionalType, DerivedType>;
protected:
///TODO: comment
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
@ -148,6 +173,8 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(children_);
}
/// @}
}; // \struct Clique
template<class DERIVED, class CONDITIONAL>

54
gtsam/inference/ClusterTree.h
View File

@ -33,6 +33,7 @@ namespace gtsam {
* A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman:
* each node k represents a subset C_k \sub X, and the tree is family preserving, in that
* each factor f_i is associated with a single cluster and scope(f_i) \sub C_k.
* \nosubgrouping
*/
template <class FG>
class ClusterTree {
@ -56,57 +57,78 @@ namespace gtsam {
public:
// Construct empty clique
/// Construct empty clique
Cluster() {}
/* Create a node with a single frontal variable */
/** Create a node with a single frontal variable */
template<typename Iterator>
Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator);
/* Create a node with several frontal variables */
/** Create a node with several frontal variables */
template<typename FRONTALIT, typename SEPARATORIT>
Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
/* Create a node with several frontal variables */
/** Create a node with several frontal variables */
template<typename FRONTALIT, typename SEPARATORIT>
Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
// print the object
/// print
void print(const std::string& indent) const;
/// print the enire tree
void printTree(const std::string& indent) const;
// check equality
/// check equality
bool equals(const Cluster& other) const;
// get or set the parent
weak_ptr& parent() { return parent_; }
// get a reference to the children
/// get a reference to the children
const std::list<shared_ptr>& children() const { return children_; }
// add a child
/// add a child
void addChild(shared_ptr child);
/// get or set the parent
weak_ptr& parent() { return parent_; }
};
// typedef for shared pointers to clusters
/// @name Advanced Interface
/// @{
/// typedef for shared pointers to clusters
typedef typename Cluster::shared_ptr sharedCluster;
// Root cluster
/// Root cluster
sharedCluster root_;
public:
// constructor of empty tree
/// @}
/// @name Standard Constructors
/// @{
/// constructor of empty tree
ClusterTree() {}
// return the root cluster
/// @}
/// @name Standard Interface
/// @{
/// return the root cluster
sharedCluster root() const { return root_; }
// print the object
/// @}
/// @name Testable
/// @{
/// print the object
void print(const std::string& str="") const;
/** check equality */
bool equals(const ClusterTree<FG>& other, double tol = 1e-9) const;
/// @}
}; // ClusterTree
} // namespace gtsam

61
gtsam/inference/Conditional.h
View File

@ -36,6 +36,7 @@ namespace gtsam {
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
* to the associated factor type and shared_ptr type of the derived class. See
* IndexConditional and GaussianConditional for examples.
* \nosubgrouping
*/
template<typename KEY>
class Conditional: public gtsam::Factor<KEY> {
@ -87,6 +88,9 @@ public:
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<const_iterator> Parents;
/// @name Standard Constructors
/// @{
/** Empty Constructor to make serialization possible */
Conditional() : nrFrontals_(0) { assertInvariants(); }
@ -102,6 +106,10 @@ public:
/** Three parents */
Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
Conditional(Key key, const std::vector<Key>& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
@ -114,11 +122,22 @@ public:
assertInvariants();
}
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string& s = "Conditional") const;
/** check equality */
template<class DERIVED>
bool equals(const DERIVED& c, double tol = 1e-9) const {
return nrFrontals_ == c.nrFrontals_ && FactorType::equals(c, tol); }
/// @}
/// @name Standard Interface
/// @{
/** return the number of frontals */
size_t nrFrontals() const { return nrFrontals_; }
@ -129,20 +148,6 @@ public:
Key firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
Key lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
/** Iterators over frontal and parent variables. */
const_iterator beginFrontals() const { return FactorType::begin(); }
const_iterator endFrontals() const { return FactorType::begin()+nrFrontals_; }
const_iterator beginParents() const { return FactorType::begin()+nrFrontals_; }
const_iterator endParents() const { return FactorType::end(); }
/** Mutable iterators and accessors */
iterator beginFrontals() { return FactorType::begin(); }
iterator endFrontals() { return FactorType::begin()+nrFrontals_; }
iterator beginParents() { return FactorType::begin()+nrFrontals_; }
iterator endParents() { return FactorType::end(); }
boost::iterator_range<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
boost::iterator_range<iterator> parents() { return boost::make_iterator_range(beginParents(), endParents()); }
/** return a view of the frontal keys */
Frontals frontals() const {
return boost::make_iterator_range(beginFrontals(), endFrontals()); }
@ -151,8 +156,29 @@ public:
Parents parents() const {
return boost::make_iterator_range(beginParents(), endParents()); }
/** print */
void print(const std::string& s = "Conditional") const;
/** Iterators over frontal and parent variables. */
const_iterator beginFrontals() const { return FactorType::begin(); } ///<TODO: comment
const_iterator endFrontals() const { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
const_iterator beginParents() const { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
const_iterator endParents() const { return FactorType::end(); } ///<TODO: comment
/// @}
/// @name Advanced Interface
/// @{
/** Mutable iterators and accessors */
iterator beginFrontals() { return FactorType::begin(); } ///<TODO: comment
iterator endFrontals() { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
iterator beginParents() { return FactorType::begin()+nrFrontals_; } ///<TODO: comment
iterator endParents() { return FactorType::end(); } ///<TODO: comment
///TODO: comment
boost::iterator_range<iterator> frontals() {
return boost::make_iterator_range(beginFrontals(), endFrontals()); }
///TODO: comment
boost::iterator_range<iterator> parents() {
return boost::make_iterator_range(beginParents(), endParents()); }
private:
/** Serialization function */
@ -162,6 +188,9 @@ private:
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
/// @}
};

39
gtsam/inference/EliminationTree.h
View File

@ -32,6 +32,7 @@ namespace gtsam {
*
* This structure is examined even more closely in a JunctionTree, which
* additionally identifies cliques in the chordal Bayes net.
* \nosubgrouping
*/
template<class FACTOR>
class EliminationTree {
@ -59,14 +60,15 @@ private:
Factors factors_; ///< factors associated with root
SubTrees subTrees_; ///< sub-trees
/// @name Standard Constructors
/// @{
/** default constructor, private, as you should use Create below */
EliminationTree(Index key = 0) : key_(key) {}
/** add a factor, for Create use only */
void add(const sharedFactor& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
/// @}
/// @name Advanced Interface
/// @{
/**
* Static internal function to build a vector of parent pointers using the
@ -74,6 +76,12 @@ private:
*/
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */
void add(const sharedFactor& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
/**
* Recursive routine that eliminates the factors arranged in an elimination tree
* @param Conditionals is a vector of shared pointers that will be modified in place
@ -106,11 +114,9 @@ public:
template<class DERIVEDFACTOR>
static shared_ptr Create(const FactorGraph<DERIVEDFACTOR>& factorGraph);
/** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ") const;
/** Test whether the tree is equal to another */
bool equals(const EliminationTree& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** Eliminate the factors to a Bayes Net
* @param function The function to use to eliminate, see the namespace functions
@ -118,6 +124,19 @@ public:
* @return The BayesNet resulting from elimination
*/
typename BayesNet::shared_ptr eliminate(Eliminate function) const;
/// @}
/// @name Testable
/// @{
/** Print the tree to cout */
void print(const std::string& name = "EliminationTree: ") const;
/** Test whether the tree is equal to another */
bool equals(const EliminationTree& other, double tol = 1e-9) const;
/// @}
};

75
gtsam/inference/Factor.h
View File

@ -48,6 +48,7 @@ template<class KEY> class Conditional;
* This class is \bold{not} virtual for performance reasons - derived symbolic classes,
* IndexFactor and IndexConditional, need to be created and destroyed quickly
* during symbolic elimination. GaussianFactor and NonlinearFactor are virtual.
* \nosubgrouping
*/
template<typename KEY>
class Factor {
@ -88,16 +89,15 @@ protected:
public:
/// @name Standard Constructors
/// @{
/** Copy constructor */
Factor(const This& f);
/** Construct from conditional, calls ConditionalType::toFactor() */
Factor(const ConditionalType& c);
/** Constructor from a collection of keys */
template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
/** Default constructor for I/O */
Factor() {}
@ -117,6 +117,10 @@ public:
Factor(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Construct n-way factor */
Factor(const std::set<Key>& keys) {
BOOST_FOREACH(const Key& key, keys) keys_.push_back(key);
@ -128,6 +132,12 @@ public:
assertInvariants();
}
/** Constructor from a collection of keys */
template<class KEYITERATOR> Factor(KEYITERATOR beginKey, KEYITERATOR endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
/// @}
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
@ -143,40 +153,54 @@ public:
typename BayesNet<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
#endif
/** iterators */
const_iterator begin() const { return keys_.begin(); }
const_iterator end() const { return keys_.end(); }
/// @name Standard Interface
/// @{
/** mutable iterators */
iterator begin() { return keys_.begin(); }
iterator end() { return keys_.end(); }
/** First key*/
/// First key
Key front() const { return keys_.front(); }
/** Last key */
/// Last key
Key back() const { return keys_.back(); }
/** find */
/// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/** print */
void print(const std::string& s = "Factor") const;
/** check equality */
bool equals(const This& other, double tol = 1e-9) const;
/**
* @return keys involved in this factor
*/
std::vector<Key>& keys() { return keys_; }
///TODO: comment
const std::vector<Key>& keys() const { return keys_; }
/** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
const_iterator end() const { return keys_.end(); } ///TODO: comment
/**
* @return the number of variables involved in this factor
*/
size_t size() const { return keys_.size(); }
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "Factor") const;
/// check equality
bool equals(const This& other, double tol = 1e-9) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* @return keys involved in this factor
*/
std::vector<Key>& keys() { return keys_; }
/** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment
iterator end() { return keys_.end(); } ///TODO: comment
private:
/** Serialization function */
@ -185,6 +209,9 @@ private:
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(keys_);
}
/// @}
};
}

42
gtsam/inference/FactorGraph.h
View File

@ -37,8 +37,7 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/**
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
* In this class, however, only factor nodes are kept around.
*
* Templated on the type of factors and values structure.
* \nosubgrouping
*/
template<class FACTOR>
class FactorGraph {
@ -69,9 +68,16 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** ------------------ Creating Factor Graphs ---------------------------- */
/// @name Standard Constructors
/// @{
/** Default constructor */
FactorGraph() {}
/// @}
/// @name Advanced Constructor
/// @{
/** convert from Bayes net */
template<class CONDITIONAL>
FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
@ -107,7 +113,9 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
template<typename DERIVEDFACTOR>
void push_back(const std::vector<boost::shared_ptr<DERIVEDFACTOR> >& factors);
/** ------------------ Querying Factor Graphs ---------------------------- */
/// @}
/// @name Testable
/// @{
/** print out graph */
void print(const std::string& s = "FactorGraph") const;
@ -115,13 +123,15 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** Check equality */
bool equals(const FactorGraph<FACTOR>& fg, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** ------------------ Querying Factor Graphs ---------------------------- */
/** const cast to the underlying vector of factors */
operator const std::vector<sharedFactor>&() const { return factors_; }
/** STL begin and end, so we can use BOOST_FOREACH */
const_iterator begin() const { return factors_.begin();}
const_iterator end() const { return factors_.end(); }
/** Get a specific factor by index */
const sharedFactor operator[](size_t i) const { assert(i<factors_.size()); return factors_[i]; }
sharedFactor& operator[](size_t i) { assert(i<factors_.size()); return factors_[i]; }
@ -171,12 +181,24 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
return ret;
}
/// @}
/// @name Advanced Interface
/// @{
/** STL begin and end, so we can use BOOST_FOREACH */
const_iterator begin() const { return factors_.begin();}
const_iterator end() const { return factors_.end(); }
/** ----------------- Modifying Factor Graphs ---------------------------- */
/** STL begin and end, so we can use BOOST_FOREACH */
iterator begin() { return factors_.begin();}
iterator end() { return factors_.end(); }
/// @}
/// @name Standard Interface
/// @{
/**
* Reserve space for the specified number of factors if you know in
* advance how many there will be (works like std::vector::reserve).
@ -195,6 +217,10 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
/** replace a factor by index */
void replace(size_t index, sharedFactor factor);
/// @}
/// @name Advanced Interface
/// @{
private:
/** Serialization function */
@ -205,6 +231,8 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
}
}; // FactorGraph
/// @}
/** Create a combined joint factor (new style for EliminationTree). */
template<class DERIVED, class KEY>
typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors,

10
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -36,6 +36,7 @@ namespace gtsam {
* JUNCTIONTREE annoyingly, you also have to supply a compatible JT type
* i.e., one templated on a factor graph with the same factors
* TODO: figure why this is so and possibly fix it
* \nosubgrouping
*/
template<class FACTOR, class JUNCTIONTREE>
class GenericMultifrontalSolver {
@ -53,6 +54,9 @@ namespace gtsam {
typedef typename FactorGraph<FACTOR>::shared_ptr sharedGraph;
typedef typename FactorGraph<FACTOR>::Eliminate Eliminate;
/// @name Standard Constructors
/// @{
/**
* Construct the solver for a factor graph. This builds the junction
* tree, which does the symbolic elimination, identifies the cliques,
@ -68,6 +72,10 @@ namespace gtsam {
GenericMultifrontalSolver(const sharedGraph& factorGraph,
const VariableIndex::shared_ptr& variableIndex);
/// @}
/// @name Standard Interface
/// @{
/**
* Replace the factor graph with a new one having the same structure. The
* This function can be used if the numerical part of the factors changes,
@ -97,6 +105,8 @@ namespace gtsam {
typename FACTOR::shared_ptr marginalFactor(Index j,
Eliminate function) const;
/// @}
};
} // gtsam

14
gtsam/inference/GenericSequentialSolver.h
View File

@ -43,6 +43,7 @@ namespace gtsam {
* However, sequential variable elimination is easier to understand so this is a good
* starting point to learn about these algorithms and our implementation.
* Additionally, the first step of MFQR is symbolic sequential elimination.
* \nosubgrouping
*/
template<class FACTOR>
class GenericSequentialSolver {
@ -73,6 +74,9 @@ namespace gtsam {
public:
/// @name Standard Constructors
/// @{
/**
* Construct the solver for a factor graph. This builds the elimination
* tree, which already does some of the work of elimination.
@ -88,12 +92,20 @@ namespace gtsam {
const sharedFactorGraph& factorGraph,
const boost::shared_ptr<VariableIndex>& variableIndex);
/// @}
/// @name Testable
/// @{
/** Print to cout */
void print(const std::string& name = "GenericSequentialSolver: ") const;
/** Test whether is equal to another */
bool equals(const GenericSequentialSolver& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/**
* Replace the factor graph with a new one having the same structure. The
* This function can be used if the numerical part of the factors changes,
@ -120,6 +132,8 @@ namespace gtsam {
*/
typename FACTOR::shared_ptr marginalFactor(Index j, Eliminate function) const;
/// @}
}; // GenericSequentialSolver
} // namespace gtsam

16
gtsam/inference/ISAM.h
View File

@ -25,6 +25,7 @@ namespace gtsam {
/**
* A Bayes tree with an update methods that implements the iSAM algorithm.
* Given a set of new factors, it re-eliminates the invalidated part of the tree.
* \nosubgrouping
*/
template<class CONDITIONAL>
class ISAM: public BayesTree<CONDITIONAL> {
@ -35,6 +36,9 @@ namespace gtsam {
public:
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */
ISAM();
@ -43,6 +47,10 @@ namespace gtsam {
Base(bayesTree) {
}
/// @}
/// @name Advanced Interface Interface
/// @{
/**
* update the Bayes tree with a set of new factors, typically derived from measurements
* @param newFactors is a factor graph that contains the new factors
@ -51,16 +59,16 @@ namespace gtsam {
template<class FG>
void update(const FG& newFactors, typename FG::Eliminate function);
/** advanced interface */
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique;
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques;
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///<TODO: comment
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques; ///<TODO: comment
/** update_internal provides access to list of orphans for drawing purposes */
template<class FG>
void update_internal(const FG& newFactors, Cliques& orphans,
typename FG::Eliminate function);
/// @}
};
}/// namespace gtsam

19
gtsam/inference/IndexConditional.h
View File

@ -31,6 +31,7 @@ namespace gtsam {
*
* It derives from Conditional with a key type of Index, which is an
* unsigned integer.
* \nosubgrouping
*/
class IndexConditional : public Conditional<Index> {
@ -46,6 +47,9 @@ namespace gtsam {
typedef IndexFactor FactorType;
typedef boost::shared_ptr<IndexConditional> shared_ptr;
/// @name Standard Constructors
/// @{
/** Empty Constructor to make serialization possible */
IndexConditional() { assertInvariants(); }
@ -61,6 +65,10 @@ namespace gtsam {
/** Three parents */
IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) { assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
IndexConditional(Index j, const std::vector<Index>& parents) : Base(j, parents) {
assertInvariants();
@ -72,6 +80,10 @@ namespace gtsam {
assertInvariants();
}
/// @}
/// @name Standard Interface
/// @{
/** Named constructor directly returning a shared pointer */
template<class KEYS>
static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals) {
@ -86,6 +98,10 @@ namespace gtsam {
return IndexFactor::shared_ptr(new IndexFactor(*this));
}
/// @}
/// @name Advanced Interface
/// @{
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
@ -105,6 +121,9 @@ namespace gtsam {
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
/// @}
};
}

36
gtsam/inference/IndexFactor.h
View File

@ -32,14 +32,20 @@ namespace gtsam {
* elimination by JunctionTree.
*
* It derives from Factor with a key type of Index, an unsigned integer.
* \nosubgrouping
*/
class IndexFactor: public Factor<Index> {
protected:
// Internal function for checking class invariants (unique keys for this factor)
/// @name Advanced Interface
/// @{
/// Internal function for checking class invariants (unique keys for this factor)
void assertInvariants() const;
/// @}
public:
typedef IndexFactor This;
@ -51,6 +57,9 @@ namespace gtsam {
/** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<IndexFactor> shared_ptr;
/// @name Standard Interface
/// @{
/** Copy constructor */
IndexFactor(const This& f) :
Base(f) {
@ -60,13 +69,6 @@ namespace gtsam {
/** Construct from derived type */
IndexFactor(const IndexConditional& c);
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey,
KeyIterator endKey) :
Base(beginKey, endKey) {
assertInvariants();
}
/** Default constructor for I/O */
IndexFactor() {
assertInvariants();
@ -96,6 +98,10 @@ namespace gtsam {
assertInvariants();
}
/// @}
/// @name Advanced Constructors
/// @{
/** Construct n-way factor */
IndexFactor(const std::set<Index>& js) :
Base(js) {
@ -108,6 +114,15 @@ namespace gtsam {
assertInvariants();
}
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey,
KeyIterator endKey) :
Base(beginKey, endKey) {
assertInvariants();
}
/// @}
#ifdef TRACK_ELIMINATE
/**
* eliminate the first variable involved in this factor
@ -120,6 +135,9 @@ namespace gtsam {
1);
#endif
/// @name Advanced Interface
/// @{
/**
* Permutes the factor, but for efficiency requires the permutation
* to already be inverted.
@ -137,6 +155,8 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
/// @}
}; // IndexFactor
}

25
gtsam/inference/JunctionTree.h
View File

@ -48,6 +48,7 @@ namespace gtsam {
*
*
* \ingroup Multifrontal
* \nosubgrouping
*/
template<class FG, class BTCLIQUE=typename BayesTree<typename FG::FactorType::ConditionalType>::Clique>
class JunctionTree: public ClusterTree<FG> {
@ -68,23 +69,33 @@ namespace gtsam {
typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
private:
// distribute the factors along the cluster tree
/// @name Advanced Interface
/// @{
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg,
const SymbolicBayesTree::sharedClique& clique);
// distribute the factors along the cluster tree
/// distribute the factors along the cluster tree
sharedClique distributeFactors(const FG& fg, const std::vector<FastList<size_t> >& targets,
const SymbolicBayesTree::sharedClique& clique);
// recursive elimination function
/// recursive elimination function
std::pair<typename BTClique::shared_ptr, typename FG::sharedFactor>
eliminateOneClique(typename FG::Eliminate function,
const boost::shared_ptr<const Clique>& clique) const;
// internal constructor
/// internal constructor
void construct(const FG& fg, const VariableIndex& variableIndex);
/// @}
public:
/// @name Standard Constructors
/// @{
/** Default constructor */
JunctionTree() {}
@ -104,6 +115,10 @@ namespace gtsam {
*/
JunctionTree(const FG& fg, const VariableIndex& variableIndex);
/// @}
/// @name Standard Interface
/// @{
/** Eliminate the factors in the subgraphs to produce a BayesTree.
* @param function The function used to eliminate, see the namespace functions
* in GaussianFactorGraph.h
@ -111,6 +126,8 @@ namespace gtsam {
*/
typename BTClique::shared_ptr eliminate(typename FG::Eliminate function) const;
/// @}
}; // JunctionTree
} // namespace gtsam

62
gtsam/inference/Permutation.h
View File

@ -42,6 +42,7 @@ class Inference;
* arguments supplied through the square-bracket [] operator through the
* permutation. Note that this helper class stores a reference to the original
* container.
* \nosubgrouping
*/
class Permutation {
protected:
@ -52,6 +53,9 @@ public:
typedef std::vector<Index>::const_iterator const_iterator;
typedef std::vector<Index>::iterator iterator;
/// @name Standard Constructors
/// @{
/**
* Create an empty permutation. This cannot do anything, but you can later
* assign to it.
@ -64,12 +68,26 @@ public:
*/
Permutation(Index nVars) : rangeIndices_(nVars) {}
/// @}
/// @name Testable
/// @{
/** Print */
void print(const std::string& str = "Permutation: ") const;
/** Check equality */
bool equals(const Permutation& rhs, double tol=0.0) const { return rangeIndices_ == rhs.rangeIndices_; }
/// @}
/// @name Standard Interface
/// @{
/**
* Permute the given variable, i.e. determine it's new index after the
* permutation.
*/
Index operator[](Index variable) const { check(variable); return rangeIndices_[variable]; }
Index& operator[](Index variable) { check(variable); return rangeIndices_[variable]; }
/**
* The number of variables in the range of this permutation, i.e. the output
@ -104,29 +122,12 @@ public:
*/
static Permutation PushToBack(const std::vector<Index>& toBack, size_t size, bool filterDuplicates = false);
iterator begin() { return rangeIndices_.begin(); }
const_iterator begin() const { return rangeIndices_.begin(); }
iterator end() { return rangeIndices_.end(); }
const_iterator end() const { return rangeIndices_.end(); }
/** Print for debugging */
void print(const std::string& str = "Permutation: ") const;
/** Equals */
bool equals(const Permutation& rhs, double tol=0.0) const { return rangeIndices_ == rhs.rangeIndices_; }
/**
* Permute the permutation, p1.permute(p2)[i] is equivalent to p1[p2[i]].
*/
Permutation::shared_ptr permute(const Permutation& permutation) const;
/**
* A partial permutation, reorders the variables selected by selector through
* partialPermutation. selector and partialPermutation should have the same
* size, this is checked if NDEBUG is not defined.
*/
Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
/**
* Return the inverse permutation. This is only possible if this is a non-
* reducing permutation, that is, (*this)[i] < this->size() for all i<size().
@ -134,9 +135,34 @@ public:
*/
Permutation::shared_ptr inverse() const;
const_iterator begin() const { return rangeIndices_.begin(); } ///<TODO: comment
const_iterator end() const { return rangeIndices_.end(); } ///<TODO: comment
/// @}
/// @name Advanced Interface
/// @{
/**
* TODO: comment
*/
Index& operator[](Index variable) { check(variable); return rangeIndices_[variable]; }
/**
* A partial permutation, reorders the variables selected by selector through
* partialPermutation. selector and partialPermutation should have the same
* size, this is checked if NDEBUG is not defined.
*/
Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
iterator begin() { return rangeIndices_.begin(); } ///<TODO: comment
iterator end() { return rangeIndices_.end(); } ///<TODO: comment
protected:
void check(Index variable) const { assert(variable < rangeIndices_.size()); }
/// @}
friend class Inference;
};

42
gtsam/inference/SymbolicFactorGraph.h
View File

@ -30,11 +30,16 @@ namespace gtsam {
typedef BayesNet<IndexConditional> SymbolicBayesNet;
typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
/** Symbolic IndexFactor Graph */
/** Symbolic IndexFactor Graph
* \nosubgrouping
*/
class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
public:
/// @name Standard Constructors
/// @{
/** Construct empty factor graph */
SymbolicFactorGraph() {
}
@ -42,6 +47,26 @@ namespace gtsam {
/** Construct from a BayesNet */
SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet);
/**
* Construct from a factor graph of any type
*/
template<class FACTOR>
SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
/// @}
/// @name Standard Interface
/// @{
/**
* Return the set of variables involved in the factors (computes a set
* union).
*/
FastSet<Index> keys() const;
/// @}
/// @name Advanced Interface
/// @{
/** Push back unary factor */
void push_factor(Index key);
@ -54,17 +79,6 @@ namespace gtsam {
/** Push back 4-way factor */
void push_factor(Index key1, Index key2, Index key3, Index key4);
/**
* Construct from a factor graph of any type
*/
template<class FACTOR>
SymbolicFactorGraph(const FactorGraph<FACTOR>& fg);
/**
* Return the set of variables involved in the factors (computes a set
* union).
*/
FastSet<Index> keys() const;
};
/** Create a combined joint factor (new style for EliminationTree). */
@ -80,7 +94,9 @@ namespace gtsam {
std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
/* Template function implementation */
/// @}
/** Template function implementation */
template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) {

54
gtsam/inference/VariableIndex.h
View File

@ -35,6 +35,7 @@ class Inference;
* from a factor graph prior to elimination, and stores the list of factors
* that involve each variable. This information is stored as a vector of
* lists of factor indices.
* \nosubgrouping
*/
class VariableIndex {
public:
@ -51,6 +52,10 @@ protected:
size_t nEntries_; // Sum of involved variable counts of each factor.
public:
/// @name Standard Constructors
/// @{
/** Default constructor, creates an empty VariableIndex */
VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
@ -67,6 +72,10 @@ public:
*/
template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph);
/// @}
/// @name Standard Interface
/// @{
/**
* The number of variable entries. This is one greater than the variable
* with the highest index.
@ -82,15 +91,24 @@ public:
/** Access a list of factors by variable */
const Factors& operator[](Index variable) const { checkVar(variable); return index_[variable]; }
/// @}
/// @name Testable
/// @{
/** Test for equality (for unit tests and debug assertions). */
bool equals(const VariableIndex& other, double tol=0.0) const;
/** Print the variable index (for unit tests and debugging). */
void print(const std::string& str = "VariableIndex: ") const;
/// @}
/// @name Advanced Interface
/// @{
/** Access a list of factors by variable */
Factors& operator[](Index variable) { checkVar(variable); return index_[variable]; }
/**
* Apply a variable permutation. Does not rearrange data, just permutes
* future lookups by variable.
*/
void permute(const Permutation& permutation);
/**
* Augment the variable index with new factors. This can be used when
* solving problems incrementally.
@ -105,22 +123,30 @@ public:
*/
template<typename CONTAINER, class FactorGraph> void remove(const CONTAINER& indices, const FactorGraph& factors);
/** Test for equality (for unit tests and debug assertions). */
bool equals(const VariableIndex& other, double tol=0.0) const;
/** Print the variable index (for unit tests and debugging). */
void print(const std::string& str = "VariableIndex: ") const;
/**
* Apply a variable permutation. Does not rearrange data, just permutes
* future lookups by variable.
*/
void permute(const Permutation& permutation);
protected:
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } ///<TODO: comment
Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } ///<TODO: comment
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); } ///<TODO: comment
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); } ///<TODO: comment
///TODO: comment
VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
///TODO: comment
void checkVar(Index variable) const { assert(variable < index_.size()); }
///TODO: comment
template<class FactorGraph> void fill(const FactorGraph& factorGraph);
Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); }
Factor_const_iterator factorsBegin(Index variable) const { checkVar(variable); return index_[variable].begin(); }
Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); }
Factor_const_iterator factorsEnd(Index variable) const { checkVar(variable); return index_[variable].end(); }
/// @}
};
/* ************************************************************************* */

11
gtsam/inference/VariableSlots.h
View File

@ -52,6 +52,7 @@ namespace gtsam {
* further sorted so that the column-block position of the first structural
* non-zero increases monotonically through the rows. This additional process
* is not performed by this class.
* \nosubgrouping
*/
class VariableSlots : public FastMap<Index, std::vector<Index> > {
@ -60,6 +61,9 @@ public:
typedef FastMap<Index, std::vector<Index> > Base;
/// @name Standard Constructors
/// @{
/**
* Constructor from a set of factors to be combined. Sorts the variables
* and keeps track of which variable from each factor ends up in each slot
@ -68,11 +72,18 @@ public:
template<class FG>
VariableSlots(const FG& factorGraph);
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string& str = "VariableSlots: ") const;
/** equals */
bool equals(const VariableSlots& rhs, double tol = 0.0) const;
/// @}
};
/* ************************************************************************* */