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

View File

@ -38,6 +38,7 @@ namespace gtsam {
* *
* todo: Symbolic using Index is a misnomer. * todo: Symbolic using Index is a misnomer.
* todo: how to handle Bayes nets with an optimize function? Currently using global functions. * todo: how to handle Bayes nets with an optimize function? Currently using global functions.
* \nosubgrouping
*/ */
template<class CONDITIONAL> template<class CONDITIONAL>
class BayesNet { class BayesNet {
@ -67,18 +68,52 @@ protected:
public: public:
/// @name Standard Constructors
/// @{
/** Default constructor as an empty BayesNet */ /** Default constructor as an empty BayesNet */
BayesNet() {}; BayesNet() {};
/** BayesNet with 1 conditional */ /** BayesNet with 1 conditional */
BayesNet(const sharedConditional& conditional) { push_back(conditional); } BayesNet(const sharedConditional& conditional) { push_back(conditional); }
/// @}
/// @name Testable
/// @{
/** print */ /** print */
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
/** check equality */ /** check equality */
bool equals(const BayesNet& other, double tol = 1e-9) const; 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 /** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this * 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 * key. Running time is approximately \f$ O(n) \f$ in the number of
@ -87,6 +122,43 @@ public:
*/ */
const_iterator find(Index key) const; 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 /** Find an iterator pointing to the conditional where the specified key
* appears as a frontal variable, or end() if no conditional contains this * 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 * key. Running time is approximately \f$ O(n) \f$ in the number of
@ -124,33 +196,6 @@ public:
*/ */
void pop_front() {conditionals_.pop_front();} 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 */ /** Permute the variables in the BayesNet */
void permuteWithInverse(const Permutation& inversePermutation); void permuteWithInverse(const Permutation& inversePermutation);
@ -161,38 +206,10 @@ public:
*/ */
bool permuteSeparatorWithInverse(const Permutation& inversePermutation); bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** size is the number of nodes */ iterator begin() {return conditionals_.begin();} ///<TODO: comment
size_t size() const { iterator end() {return conditionals_.end();} ///<TODO: comment
return conditionals_.size(); reverse_iterator rbegin() {return conditionals_.rbegin();} ///<TODO: comment
} reverse_iterator rend() {return conditionals_.rend();} ///<TODO: comment
/** 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();}
/** saves the bayes to a text file in GraphViz format */ /** saves the bayes to a text file in GraphViz format */
// void saveGraph(const std::string& s) const; // void saveGraph(const std::string& s) const;
@ -204,6 +221,8 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_); ar & BOOST_SERIALIZATION_NVP(conditionals_);
} }
/// @}
}; // BayesNet }; // BayesNet
} // namespace gtsam } // namespace gtsam

View File

@ -46,6 +46,7 @@ namespace gtsam {
* as it is only used when developing special versions of BayesTree, e.g. for ISAM2. * as it is only used when developing special versions of BayesTree, e.g. for ISAM2.
* *
* \ingroup Multifrontal * \ingroup Multifrontal
* \nosubgrouping
*/ */
template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> > template<class CONDITIONAL, class CLIQUE=BayesTreeClique<CONDITIONAL> >
class BayesTree { class BayesTree {
@ -128,12 +129,19 @@ namespace gtsam {
public: public:
/// @name Standard Constructors
/// @{
/** Create an empty Bayes Tree */ /** Create an empty Bayes Tree */
BayesTree(); BayesTree();
/** Create a Bayes Tree from a Bayes Net */ /** Create a Bayes Tree from a Bayes Net */
BayesTree(const BayesNet<CONDITIONAL>& bayesNet); 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 * 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. * new root clique and the subtrees are connected to the root clique.
@ -143,53 +151,19 @@ namespace gtsam {
/** Destructor */ /** Destructor */
virtual ~BayesTree() {} virtual ~BayesTree() {}
/** /// @}
* Constructing Bayes trees /// @name Testable
*/ /// @{
/** 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
*/
/** check equality */ /** check equality */
bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const; bool equals(const BayesTree<CONDITIONAL,CLIQUE>& other, double tol = 1e-9) const;
void cloneTo(shared_ptr& newTree) const { /** print */
cloneTo(newTree, root(), sharedClique()); void print(const std::string& s = "") const;
}
private: /// @}
/** deep copy from another tree */ /// @name Standard Interface
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);
}
}
public: public:
@ -214,9 +188,6 @@ namespace gtsam {
/** return root clique */ /** return root clique */
const sharedClique& root() const { return root_; } const sharedClique& root() const { return root_; }
/** Access the root clique (non-const version) */
sharedClique& root() { return root_; }
/** find the clique to which key belongs */ /** find the clique to which key belongs */
sharedClique operator[](Index key) const { sharedClique operator[](Index key) const {
return nodes_.at(key); return nodes_.at(key);
@ -245,15 +216,15 @@ namespace gtsam {
* Read only with side effects * Read only with side effects
*/ */
/** print */
void print(const std::string& s = "") const;
/** saves the Tree to a text file in GraphViz format */ /** saves the Tree to a text file in GraphViz format */
void saveGraph(const std::string& s) const; 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 */ /** Remove all nodes */
void clear(); void clear();
@ -271,7 +242,45 @@ namespace gtsam {
template<class CONTAINER> template<class CONTAINER>
void removeTop(const CONTAINER& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans); 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: 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 */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -280,6 +289,8 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(root_); ar & BOOST_SERIALIZATION_NVP(root_);
} }
/// @}
}; // BayesTree }; // BayesTree

View File

@ -41,6 +41,7 @@ namespace gtsam {
* *
* @tparam DERIVED The derived clique type. * @tparam DERIVED The derived clique type.
* @tparam CONDITIONAL The conditional type. * @tparam CONDITIONAL The conditional type.
* \nosubgrouping
*/ */
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>
struct BayesTreeCliqueBase { struct BayesTreeCliqueBase {
@ -58,7 +59,9 @@ namespace gtsam {
typedef typename FactorGraph<FactorType>::Eliminate Eliminate; typedef typename FactorGraph<FactorType>::Eliminate Eliminate;
protected: protected:
void assertInvariants() const;
/// @name Standard Constructors
/// @{
/** Default constructor */ /** Default constructor */
BayesTreeCliqueBase() {} BayesTreeCliqueBase() {}
@ -69,11 +72,57 @@ namespace gtsam {
/** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */ /** Construct from an elimination result, which is a pair<CONDITIONAL,FACTOR> */
BayesTreeCliqueBase(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result); BayesTreeCliqueBase(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result);
/// @}
public: public:
sharedConditional conditional_; sharedConditional conditional_;
derived_weak_ptr parent_; derived_weak_ptr parent_;
std::list<derived_ptr> children_; 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 */ /** 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); } 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. * types, such as ISAM2Clique, the factor part is kept as a cached factor.
* @param An elimination result, which is a pair<CONDITIONAL,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_))); } ///TODO: comment
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;
/** Permute the variables in the whole subtree rooted at this clique */ /** Permute the variables in the whole subtree rooted at this clique */
void permuteWithInverse(const Permutation& inversePermutation); void permuteWithInverse(const Permutation& inversePermutation);
@ -131,13 +156,13 @@ namespace gtsam {
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */ /** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function); 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>; friend class BayesTree<ConditionalType, DerivedType>;
protected:
///TODO: comment
void assertInvariants() const;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -148,6 +173,8 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(children_); ar & BOOST_SERIALIZATION_NVP(children_);
} }
/// @}
}; // \struct Clique }; // \struct Clique
template<class DERIVED, class CONDITIONAL> template<class DERIVED, class CONDITIONAL>

View File

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

View File

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

View File

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

View File

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

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -42,6 +42,7 @@ class Inference;
* arguments supplied through the square-bracket [] operator through the * arguments supplied through the square-bracket [] operator through the
* permutation. Note that this helper class stores a reference to the original * permutation. Note that this helper class stores a reference to the original
* container. * container.
* \nosubgrouping
*/ */
class Permutation { class Permutation {
protected: protected:
@ -52,6 +53,9 @@ public:
typedef std::vector<Index>::const_iterator const_iterator; typedef std::vector<Index>::const_iterator const_iterator;
typedef std::vector<Index>::iterator iterator; typedef std::vector<Index>::iterator iterator;
/// @name Standard Constructors
/// @{
/** /**
* Create an empty permutation. This cannot do anything, but you can later * Create an empty permutation. This cannot do anything, but you can later
* assign to it. * assign to it.
@ -64,12 +68,26 @@ public:
*/ */
Permutation(Index nVars) : rangeIndices_(nVars) {} 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 * Permute the given variable, i.e. determine it's new index after the
* permutation. * permutation.
*/ */
Index operator[](Index variable) const { check(variable); return rangeIndices_[variable]; } 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 * 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); 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]]. * Permute the permutation, p1.permute(p2)[i] is equivalent to p1[p2[i]].
*/ */
Permutation::shared_ptr permute(const Permutation& permutation) const; 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- * 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(). * reducing permutation, that is, (*this)[i] < this->size() for all i<size().
@ -134,9 +135,34 @@ public:
*/ */
Permutation::shared_ptr inverse() const; 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: protected:
void check(Index variable) const { assert(variable < rangeIndices_.size()); } void check(Index variable) const { assert(variable < rangeIndices_.size()); }
/// @}
friend class Inference; friend class Inference;
}; };

View File

@ -30,11 +30,16 @@ namespace gtsam {
typedef BayesNet<IndexConditional> SymbolicBayesNet; typedef BayesNet<IndexConditional> SymbolicBayesNet;
typedef EliminationTree<IndexFactor> SymbolicEliminationTree; typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
/** Symbolic IndexFactor Graph */ /** Symbolic IndexFactor Graph
* \nosubgrouping
*/
class SymbolicFactorGraph: public FactorGraph<IndexFactor> { class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
public: public:
/// @name Standard Constructors
/// @{
/** Construct empty factor graph */ /** Construct empty factor graph */
SymbolicFactorGraph() { SymbolicFactorGraph() {
} }
@ -42,6 +47,26 @@ namespace gtsam {
/** Construct from a BayesNet */ /** Construct from a BayesNet */
SymbolicFactorGraph(const BayesNet<IndexConditional>& 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 */ /** Push back unary factor */
void push_factor(Index key); void push_factor(Index key);
@ -54,17 +79,6 @@ namespace gtsam {
/** Push back 4-way factor */ /** Push back 4-way factor */
void push_factor(Index key1, Index key2, Index key3, Index key4); 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). */ /** 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> > std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1); EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
/* Template function implementation */ /// @}
/** Template function implementation */
template<class FACTOR> template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) { SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) { for (size_t i = 0; i < fg.size(); i++) {

View File

@ -35,6 +35,7 @@ class Inference;
* from a factor graph prior to elimination, and stores the list of factors * 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 * that involve each variable. This information is stored as a vector of
* lists of factor indices. * lists of factor indices.
* \nosubgrouping
*/ */
class VariableIndex { class VariableIndex {
public: public:
@ -51,6 +52,10 @@ protected:
size_t nEntries_; // Sum of involved variable counts of each factor. size_t nEntries_; // Sum of involved variable counts of each factor.
public: public:
/// @name Standard Constructors
/// @{
/** Default constructor, creates an empty VariableIndex */ /** Default constructor, creates an empty VariableIndex */
VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {} VariableIndex() : index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
@ -67,6 +72,10 @@ public:
*/ */
template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph); template<class FactorGraph> VariableIndex(const FactorGraph& factorGraph);
/// @}
/// @name Standard Interface
/// @{
/** /**
* The number of variable entries. This is one greater than the variable * The number of variable entries. This is one greater than the variable
* with the highest index. * with the highest index.
@ -82,15 +91,24 @@ public:
/** Access a list of factors by variable */ /** Access a list of factors by variable */
const Factors& operator[](Index variable) const { checkVar(variable); return index_[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 */ /** Access a list of factors by variable */
Factors& operator[](Index variable) { checkVar(variable); return index_[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 * Augment the variable index with new factors. This can be used when
* solving problems incrementally. * solving problems incrementally.
@ -105,22 +123,30 @@ public:
*/ */
template<typename CONTAINER, class FactorGraph> void remove(const CONTAINER& indices, const FactorGraph& factors); 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: 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) {} VariableIndex(size_t nVars) : indexUnpermuted_(nVars), index_(indexUnpermuted_), nFactors_(0), nEntries_(0) {}
///TODO: comment
void checkVar(Index variable) const { assert(variable < index_.size()); } void checkVar(Index variable) const { assert(variable < index_.size()); }
///TODO: comment
template<class FactorGraph> void fill(const FactorGraph& factorGraph); 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(); }
}; };
/* ************************************************************************* */ /* ************************************************************************* */

View File

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