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

Documentation for FactorBase, ConditionalBase, IndexFactor, IndexConditional

release/4.3a0
Richard Roberts 2011-02-15 16:22:35 +00:00
parent 09f25edcbb
commit 62b3db9535
6 changed files with 220 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
gtsam/inference/ConditionalBase.h
View File

@ -32,7 +32,15 @@
namespace gtsam {
/**
* Base class for conditional densities
* Base class for conditional densities, templated on KEY type. This class
* provides storage for the keys involved in a conditional, and iterators and
* access to the frontal and separator keys.
*
* todo: Move permutation functions to IndexConditional.
*
* Derived classes *must* redefine the Factor and shared_ptr typedefs to refer
* to the associated factor type and shared_ptr type of the derived class. See
* IndexConditional and GaussianConditional for examples.
*
* We make it noncopyable so we enforce the fact that factors are
* kept in pointer containers. To be safe, you should make them
@ -46,15 +54,36 @@ protected:
/** The first nFrontal variables are frontal and the rest are parents. */
size_t nrFrontals_;
/** Debugging invariant that the keys should be in order, including that the
* conditioned variable is numbered lower than the parents.
*/
void assertInvariants() const;
public:
typedef KEY Key;
typedef ConditionalBase<Key> This;
/**
* Typedef to the factor type that produces this conditional and that this
* conditional can be converted to using a factor constructor. Derived
* classes must redefine this.
*/
typedef gtsam::FactorBase<Key> Factor;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr<This> shared_ptr;
/** Iterator over keys */
typedef typename Factor::iterator iterator;
/** Const iterator over keys */
typedef typename Factor::const_iterator const_iterator;
/** View of the frontal keys (call frontals()) */
typedef boost::iterator_range<const_iterator> Frontals;
/** View of the separator keys (call parents()) */
typedef boost::iterator_range<const_iterator> Parents;
/** Empty Constructor to make serialization possible */
@ -135,62 +164,69 @@ public:
return boost::make_iterator_range(beginParents(), endParents()); }
/** print */
void print(const std::string& s = "Conditional") const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals()) std::cout << " " << key;
if (nrParents()>0) std::cout << " |";
BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl;
}
void print(const std::string& s = "Conditional") const;
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(Key& parent, parents()) {
Key newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
}
}
return parentChanged;
}
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/**
* Permutes the Conditional, but for efficiency requires the permutation
* to already be inverted.
*/
void permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}
#endif
Factor::permuteWithInverse(inversePermutation);
}
protected:
/** Debugging invariant that the keys should be in order, including that the
* conditioned variable is numbered lower than the parents.
*/
void assertInvariants() const;
void permuteWithInverse(const Permutation& inversePermutation);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
}
};
/* ************************************************************************* */
template<typename KEY>
void ConditionalBase<KEY>::print(const std::string& s) const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals()) std::cout << " " << key;
if (nrParents()>0) std::cout << " |";
BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl;
}
/* ************************************************************************* */
template<typename KEY>
bool ConditionalBase<KEY>::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(Key& parent, parents()) {
Key newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
}
}
return parentChanged;
}
/* ************************************************************************* */
template<typename KEY>
void ConditionalBase<KEY>::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}
#endif
Factor::permuteWithInverse(inversePermutation);
}
}

3
gtsam/inference/FactorBase-inl.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file Factor-inl.h
* @file FactorBase-inl.h
* @brief
* @author Richard Roberts
* @created Sep 1, 2010
@ -57,7 +57,6 @@ void FactorBase<KEY>::print(const std::string& s) const {
/* ************************************************************************* */
template<typename KEY>
//template<class DERIVED>
bool FactorBase<KEY>::equals(const This& other, double tol) const {
return keys_ == other.keys_;
}

46
gtsam/inference/FactorBase.h
View File

@ -10,11 +10,11 @@
* -------------------------------------------------------------------------- */
/**
* @file Factor.h
* @brief A simple factor class to use in a factor graph
* @brief factor
* @file FactorBase.h
* @brief The base class for all factors
* @author Kai Ni
* @author Frank Dellaert
* @author Richard Roberts
*/
// \callgraph
@ -36,18 +36,17 @@ namespace gtsam {
template<class KEY> class ConditionalBase;
/**
* A simple factor class to use in a factor graph.
* This is the base class for all factor types. It is templated on a KEY type,
* though currently only IndexFactor and IndexConditional derive from this
* class, using Index keys. This class does not store any data other than its
* keys. Derived classes store data such as matrices and probability tables.
*
* We make it noncopyable so we enforce the fact that factors are
* kept in pointer containers. To be safe, you should make them
* immutable, i.e., practicing functional programming. However, this
* conflicts with efficiency as well, esp. in the case of incomplete
* QR factorization. A solution is still being sought.
* todo: Make NonlinearFactor derive from this too, which requires moving
* Combine, eliminate*, permute* and the sorted key invariant to IndexFactor.
*
* A Factor is templated on a Values, for example VectorValues is a values structure of
* labeled vectors. This way, we can have factors that might be defined on discrete
* variables, continuous ones, or a combination of both. It is up to the config to
* provide the appropriate values at the appropriate time.
* Note that derived classes *must* redefine the Conditional and shared_ptr
* typedefs to refer to the associated conditional and shared_ptr types of the
* derived class. See IndexFactor, JacobianFactor, etc. for examples.
*/
template<typename KEY>
class FactorBase : public Testable<FactorBase<KEY> > {
@ -56,17 +55,29 @@ public:
typedef KEY Key;
typedef FactorBase<Key> This;
/**
* Typedef to the conditional type obtained by eliminating this factor.
* Derived classes must redefine this.
*/
typedef gtsam::ConditionalBase<Key> Conditional;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr<FactorBase> shared_ptr;
/** Iterator over keys */
typedef std::vector<Index>::iterator iterator;
/** Const iterator over keys */
typedef std::vector<Index>::const_iterator const_iterator;
protected:
// The keys involved in this factor
std::vector<Key> keys_;
/** Internal check to make sure keys are sorted.
* If NDEBUG is defined, this is empty and optimized out. */
// Internal check to make sure keys are sorted. If NDEBUG is defined, this
// is empty and optimized out.
void assertInvariants() const;
public:
@ -150,16 +161,15 @@ public:
void print(const std::string& s = "Factor") const;
/** check equality */
// template<class DERIVED>
bool equals(const This& other, double tol = 1e-9) const;
/**
* return keys in order as created
* @return keys involved in this factor
*/
const std::vector<Key>& keys() const { return keys_; }
/**
* @return the number of nodes the factor connects
* @return the number of variables involved in this factor
*/
size_t size() const { return keys_.size(); }

66
gtsam/inference/IndexConditional.h
View File

@ -23,46 +23,54 @@
namespace gtsam {
class IndexConditional : public ConditionalBase<Index> {
/**
* IndexConditional serves two purposes. It is the base class for
* GaussianConditional, and also functions as a symbolic conditional with
* Index keys, produced by symbolic elimination of IndexFactor.
*
* It derives from ConditionalBase with a key type of Index, which is an
* unsigned integer.
*/
class IndexConditional : public ConditionalBase<Index> {
public:
public:
typedef IndexConditional This;
typedef ConditionalBase<Index> Base;
typedef IndexFactor Factor;
typedef boost::shared_ptr<IndexConditional> shared_ptr;
typedef IndexConditional This;
typedef ConditionalBase<Index> Base;
typedef IndexFactor Factor;
typedef boost::shared_ptr<IndexConditional> shared_ptr;
/** Empty Constructor to make serialization possible */
IndexConditional() {}
/** Empty Constructor to make serialization possible */
IndexConditional() {}
/** No parents */
IndexConditional(Index j) : Base(j) {}
/** No parents */
IndexConditional(Index j) : Base(j) {}
/** Single parent */
IndexConditional(Index j, Index parent) : Base(j, parent) {}
/** Single parent */
IndexConditional(Index j, Index parent) : Base(j, parent) {}
/** Two parents */
IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) {}
/** Two parents */
IndexConditional(Index j, Index parent1, Index parent2) : Base(j, parent1, parent2) {}
/** Three parents */
IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) {}
/** Three parents */
IndexConditional(Index j, Index parent1, Index parent2, Index parent3) : Base(j, parent1, parent2, parent3) {}
/** Constructor from a frontal variable and a vector of parents */
IndexConditional(Index j, const std::vector<Index>& parents) : Base(j, parents) {}
/** Constructor from a frontal variable and a vector of parents */
IndexConditional(Index j, const std::vector<Index>& parents) : Base(j, parents) {}
/** Constructor from a frontal variable and an iterator range of parents */
template<typename ITERATOR>
static shared_ptr FromRange(Index j, ITERATOR firstParent, ITERATOR lastParent) {
return Base::FromRange<This>(j, firstParent, lastParent); }
/** Constructor from a frontal variable and an iterator range of parents */
template<typename ITERATOR>
static shared_ptr FromRange(Index j, ITERATOR firstParent, ITERATOR lastParent) {
return Base::FromRange<This>(j, firstParent, lastParent); }
/** Named constructor from any number of frontal variables and parents */
template<typename ITERATOR>
static shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) {
return Base::FromRange<This>(firstKey, lastKey, nrFrontals); }
/** Named constructor from any number of frontal variables and parents */
template<typename ITERATOR>
static shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) {
return Base::FromRange<This>(firstKey, lastKey, nrFrontals); }
/** Convert to a factor */
IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); }
/** Convert to a factor */
IndexFactor::shared_ptr toFactor() const { return IndexFactor::shared_ptr(new IndexFactor(*this)); }
};
};
}

127
gtsam/inference/IndexFactor.h
View File

@ -22,66 +22,85 @@
namespace gtsam {
class IndexConditional;
class IndexFactor : public FactorBase<Index> {
public:
typedef IndexFactor This;
typedef FactorBase<Index> Base;
typedef IndexConditional Conditional;
typedef boost::shared_ptr<IndexFactor> shared_ptr;
/** Copy constructor */
IndexFactor(const This& f) : Base(f) {}
/** Construct from derived type */
IndexFactor(const IndexConditional& c);
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey, KeyIterator endKey) :
Base(beginKey, endKey) {}
/** Default constructor for I/O */
IndexFactor() {}
/** Construct unary factor */
IndexFactor(Index j) : Base(j) {}
/** Construct binary factor */
IndexFactor(Index j1, Index j2) : Base(j1, j2) {}
/** Construct ternary factor */
IndexFactor(Index j1, Index j2, Index j3) : Base(j1, j2, j3) {}
/** Construct 4-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) {}
/** Construct n-way factor */
IndexFactor(const std::set<Index>& js) : Base(js) {}
// Forward declaration of IndexConditional
class IndexConditional;
/**
* Combine and eliminate several factors.
* IndexFactor serves two purposes. It is the base class for all linear
* factors (GaussianFactor, JacobianFactor, HessianFactor), and also
* functions as a symbolic factor with Index keys, used to do symbolic
* elimination by JunctionTree.
*
* This class provides symbolic elimination, via the CombineAndEliminate
* function. Combine and eliminate can also be called separately, but for
* this and derived classes calling them separately generally does extra
* work.
*
* It derives from FactorBase with a key type of Index, which is an unsigned
* integer.
*/
static std::pair<BayesNet<Conditional>::shared_ptr, shared_ptr> CombineAndEliminate(
const FactorGraph<This>& factors, size_t nrFrontals=1);
class IndexFactor : public FactorBase<Index> {
/** Create a combined joint factor (new style for EliminationTree). */
static shared_ptr
Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots);
public:
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
boost::shared_ptr<Conditional> eliminateFirst();
typedef IndexFactor This;
typedef FactorBase<Index> Base;
/**
* eliminate the first nrFrontals frontal variables.
*/
boost::shared_ptr<BayesNet<Conditional> > eliminate(size_t nrFrontals = 1);
/** Elimination produces an IndexConditional */
typedef IndexConditional Conditional;
};
/** Overriding the shared_ptr typedef */
typedef boost::shared_ptr<IndexFactor> shared_ptr;
/** Copy constructor */
IndexFactor(const This& f) : Base(f) {}
/** Construct from derived type */
IndexFactor(const IndexConditional& c);
/** Constructor from a collection of keys */
template<class KeyIterator> IndexFactor(KeyIterator beginKey, KeyIterator endKey) :
Base(beginKey, endKey) {}
/** Default constructor for I/O */
IndexFactor() {}
/** Construct unary factor */
IndexFactor(Index j) : Base(j) {}
/** Construct binary factor */
IndexFactor(Index j1, Index j2) : Base(j1, j2) {}
/** Construct ternary factor */
IndexFactor(Index j1, Index j2, Index j3) : Base(j1, j2, j3) {}
/** Construct 4-way factor */
IndexFactor(Index j1, Index j2, Index j3, Index j4) : Base(j1, j2, j3, j4) {}
/** Construct n-way factor */
IndexFactor(const std::set<Index>& js) : Base(js) {}
/**
* Combine and eliminate several factors.
*/
static std::pair<BayesNet<Conditional>::shared_ptr, shared_ptr> CombineAndEliminate(
const FactorGraph<This>& factors, size_t nrFrontals=1);
/** Create a combined joint factor (new style for EliminationTree). */
static shared_ptr
Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots);
/**
* eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable
*/
boost::shared_ptr<Conditional> eliminateFirst();
/**
* eliminate the first nrFrontals frontal variables.
*/
boost::shared_ptr<BayesNet<Conditional> > eliminate(size_t nrFrontals = 1);
};
}

2
gtsam/linear/JacobianFactor.cpp
View File

@ -52,7 +52,7 @@ namespace gtsam {
/* ************************************************************************* */
inline void JacobianFactor::assertInvariants() const {
#ifndef NDEBUG
IndexFactor::assertInvariants();
IndexFactor::assertInvariants(); // The base class checks for sorted keys
assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks());
assert(firstNonzeroBlocks_.size() == Ab_.size1());
for(size_t i=0; i<firstNonzeroBlocks_.size(); ++i)