added comment groupings to inference
parent
a8607d284d
commit
f7865c80b1
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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>
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -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_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -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++) {
|
||||||
|
|
|
@ -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(); }
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
Loading…
Reference in New Issue