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

Change that may be reverted again soon - templated factor base type on key and make IndexFactor and IndexConditional for all current code. Did this to experiment with using symbols on the linear side in an efficient way but am putting this on hold for now because of complications. Will revisit and either fix or revert to int-only later in the week. Also moved some files to experimental and templating EliminationTree on FACTOR instead of FACTORGRAPH.

release/4.3a0
Richard Roberts 2010-10-19 21:31:13 +00:00
parent 01f73815e6
commit f15fea202e
47 changed files with 827 additions and 898 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
base/FastMap.h Normal file
View File

@ -0,0 +1,55 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FastMap.h
* @brief A thin wrapper around std::map that uses boost's fast_pool_allocator.
* @author Richard Roberts
* @created Oct 17, 2010
*/
#pragma once
#include <map>
#include <boost/pool/pool_alloc.hpp>
namespace gtsam {
/**
* FastMap is a thin wrapper around std::map that uses the boost
* fast_pool_allocator instead of the default STL allocator. This is just a
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several
* percent.
*/
template<typename KEY, typename VALUE>
class FastMap : public std::map<KEY, VALUE, std::less<KEY>, boost::fast_pool_allocator<std::pair<const KEY, VALUE> > > {
public:
typedef std::map<KEY, VALUE, std::less<KEY>, boost::fast_pool_allocator<std::pair<const KEY, VALUE> > > Base;
/** Default constructor */
FastMap() {}
/** Constructor from a range, passes through to base class */
template<typename InputIterator>
FastMap(InputIterator first, InputIterator last) : Base(first, last) {}
/** Copy constructor from another FastMap */
FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}
/** Copy constructor from the base map class */
FastMap(const Base& x) : Base(x) {}
};
}

52
base/FastSet.h Normal file
View File

@ -0,0 +1,52 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file FastSet.h
* @brief A thin wrapper around std::set that uses boost's fast_pool_allocator.
* @author Richard Roberts
* @created Oct 17, 2010
*/
#pragma once
#include <map>
#include <boost/pool/pool_alloc.hpp>
namespace gtsam {
/**
* FastSet is a thin wrapper around std::set that uses the boost
* fast_pool_allocator instead of the default STL allocator. This is just a
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several
* percent.
*/
template<typename VALUE>
class FastSet: public std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > {
public:
typedef std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > Base;
/** Constructor from a range, passes through to base class */
template<typename InputIterator>
FastSet(InputIterator first, InputIterator last) : Base(first, last) {}
/** Copy constructor from another FastMap */
FastSet(const FastSet<VALUE>& x) : Base(x) {}
/** Copy constructor from the base map class */
FastSet(const Base& x) : Base(x) {}
};
}

126
inference/BinaryConditional.h
View File

@ -1,126 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file DiscreteConditional.h
* @brief Discrete Conditional node for use in Bayes nets
* @author Manohar Paluri
*/
// \callgraph
#pragma once
#include <list>
#include <string>
#include <iostream>
#include <boost/shared_ptr.hpp>
#include <boost/foreach.hpp> // TODO: make cpp file
#include <boost/serialization/list.hpp>
#include <boost/serialization/vector.hpp>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/SymbolMap.h>
namespace gtsam {
/**
* Conditional node for use in a Bayes net
*/
class BinaryConditional: public Conditional {
private:
std::list<Symbol> parents_;
std::vector<double> cpt_;
public:
/** convenience typename for a shared pointer to this class */
typedef boost::shared_ptr<BinaryConditional> shared_ptr;
/**
* Empty Constructor to make serialization possible
*/
BinaryConditional(){}
/**
* No parents
*/
BinaryConditional(const Symbol& key, double p) :
Conditional(key) {
cpt_.push_back(1-p);
cpt_.push_back(p);
}
/**
* Single parent
*/
BinaryConditional(const Symbol& key, const Symbol& parent, const std::vector<double>& cpt) :
Conditional(key) {
parents_.push_back(parent);
for( size_t i = 0 ; i < cpt.size() ; i++ )
cpt_.push_back(1-cpt[i]); // p(!x|parents)
cpt_.insert(cpt_.end(),cpt.begin(),cpt.end()); // p(x|parents)
}
double probability( SymbolMap<bool> config) {
int index = 0, count = 1;
BOOST_FOREACH(const Symbol& parent, parents_){
index += count*(int)(config[parent]);
count = count << 1;
}
if( config.at(key_) )
index += count;
return cpt_[index];
}
/** print */
void print(const std::string& s = "BinaryConditional") const {
std::cout << s << " P(" << (std::string)key_;
if (parents_.size()>0) std::cout << " |";
BOOST_FOREACH(std::string parent, parents_) std::cout << " " << parent;
std::cout << ")" << std::endl;
std::cout << "Conditional Probability Table::" << std::endl;
BOOST_FOREACH(double p, cpt_) std::cout << p << "\t";
std::cout<< std::endl;
}
/** check equality */
bool equals(const Conditional& c, double tol = 1e-9) const {
if (!Conditional::equals(c)) return false;
const BinaryConditional* p = dynamic_cast<const BinaryConditional*> (&c);
if (p == NULL) return false;
return (parents_ == p->parents_ && cpt_ == p->cpt_);
}
/** return parents */
std::list<Symbol> parents() const { return parents_;}
/** return Conditional probability table*/
std::vector<double> cpt() const { return cpt_;}
/** find the number of parents */
size_t nrParents() const {
return parents_.size();
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Conditional", boost::serialization::base_object<Conditional>(*this));
ar & BOOST_SERIALIZATION_NVP(parents_);
ar & BOOST_SERIALIZATION_NVP(cpt_);
}
};
} /// namespace gtsam

79
inference/Conditional.h
View File

@ -38,7 +38,8 @@ namespace gtsam {
* kept in pointer containers. To be safe, you should make them * kept in pointer containers. To be safe, you should make them
* immutable, i.e., practicing functional programming. * immutable, i.e., practicing functional programming.
*/ */
class Conditional: public Factor, boost::noncopyable, public Testable<Conditional> { template<typename KEY>
class ConditionalBase: public gtsam::FactorBase<KEY>, boost::noncopyable, public Testable<ConditionalBase<KEY> > {
protected: protected:
@ -47,40 +48,41 @@ protected:
public: public:
/** convenience typename for a shared pointer to this class */ typedef KEY Key;
typedef gtsam::Factor Factor; typedef ConditionalBase<Key> This;
typedef boost::shared_ptr<Conditional> shared_ptr; typedef gtsam::FactorBase<Key> Factor;
typedef Factor::iterator iterator; typedef boost::shared_ptr<This> shared_ptr;
typedef Factor::const_iterator const_iterator; typedef typename Factor::iterator iterator;
typedef typename Factor::const_iterator const_iterator;
typedef boost::iterator_range<const_iterator> Frontals; typedef boost::iterator_range<const_iterator> Frontals;
typedef boost::iterator_range<const_iterator> Parents; typedef boost::iterator_range<const_iterator> Parents;
/** Empty Constructor to make serialization possible */ /** Empty Constructor to make serialization possible */
Conditional() : nrFrontals_(0) {} ConditionalBase() : nrFrontals_(0) {}
/** No parents */ /** No parents */
Conditional(Index key) : Factor(key), nrFrontals_(1) {} ConditionalBase(Key key) : Factor(key), nrFrontals_(1) {}
/** Single parent */ /** Single parent */
Conditional(Index key, Index parent) : Factor(key, parent), nrFrontals_(1) {} ConditionalBase(Key key, Key parent) : Factor(key, parent), nrFrontals_(1) {}
/** Two parents */ /** Two parents */
Conditional(Index key, Index parent1, Index parent2) : Factor(key, parent1, parent2), nrFrontals_(1) {} ConditionalBase(Key key, Key parent1, Key parent2) : Factor(key, parent1, parent2), nrFrontals_(1) {}
/** Three parents */ /** Three parents */
Conditional(Index key, Index parent1, Index parent2, Index parent3) : Factor(key, parent1, parent2, parent3), nrFrontals_(1) {} ConditionalBase(Key key, Key parent1, Key parent2, Key parent3) : Factor(key, parent1, parent2, parent3), nrFrontals_(1) {}
/** Constructor from a frontal variable and a vector of parents */ /** Constructor from a frontal variable and a vector of parents */
Conditional(Index key, const std::vector<Index>& parents) : nrFrontals_(1) { ConditionalBase(Key key, const std::vector<Key>& parents) : nrFrontals_(1) {
keys_.resize(1 + parents.size()); Factor::keys_.resize(1 + parents.size());
*(beginFrontals()) = key; *(beginFrontals()) = key;
std::copy(parents.begin(), parents.end(), beginParents()); std::copy(parents.begin(), parents.end(), beginParents());
} }
/** Constructor from a frontal variable and an iterator range of parents */ /** Constructor from a frontal variable and an iterator range of parents */
template<typename Iterator> template<class DERIVED, typename ITERATOR>
static Conditional::shared_ptr FromRange(Index key, Iterator firstParent, Iterator lastParent) { static typename DERIVED::shared_ptr FromRange(Key key, ITERATOR firstParent, ITERATOR lastParent) {
Conditional::shared_ptr conditional(new Conditional); typename DERIVED::shared_ptr conditional(new DERIVED);
conditional->nrFrontals_ = 1; conditional->nrFrontals_ = 1;
conditional->keys_.push_back(key); conditional->keys_.push_back(key);
std::copy(firstParent, lastParent, back_inserter(conditional->keys_)); std::copy(firstParent, lastParent, back_inserter(conditional->keys_));
@ -88,38 +90,39 @@ public:
} }
/** Named constructor from any number of frontal variables and parents */ /** Named constructor from any number of frontal variables and parents */
template<typename Iterator> template<typename DERIVED, typename ITERATOR>
static Conditional::shared_ptr FromRange(Iterator firstKey, Iterator lastKey, size_t nrFrontals) { static typename DERIVED::shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) {
Conditional::shared_ptr conditional(new Conditional); typename DERIVED::shared_ptr conditional(new DERIVED);
conditional->nrFrontals_ = nrFrontals; conditional->nrFrontals_ = nrFrontals;
std::copy(firstKey, lastKey, back_inserter(conditional->keys_)); std::copy(firstKey, lastKey, back_inserter(conditional->keys_));
return conditional; return conditional;
} }
/** check equality */ /** check equality */
bool equals(const Conditional& c, double tol = 1e-9) const { template<class DERIVED>
bool equals(const DERIVED& c, double tol = 1e-9) const {
return nrFrontals_ == c.nrFrontals_ && Factor::equals(c, tol); } return nrFrontals_ == c.nrFrontals_ && Factor::equals(c, tol); }
/** return the number of frontals */ /** return the number of frontals */
size_t nrFrontals() const { return nrFrontals_; } size_t nrFrontals() const { return nrFrontals_; }
/** return the number of parents */ /** return the number of parents */
size_t nrParents() const { return keys_.size() - nrFrontals_; } size_t nrParents() const { return Factor::keys_.size() - nrFrontals_; }
/** Special accessor when there is only one frontal variable. */ /** Special accessor when there is only one frontal variable. */
Index key() const { assert(nrFrontals_==1); return keys_[0]; } Key key() const { assert(nrFrontals_==1); return Factor::keys_[0]; }
/** Iterators over frontal and parent variables. */ /** Iterators over frontal and parent variables. */
const_iterator beginFrontals() const { return keys_.begin(); } const_iterator beginFrontals() const { return Factor::keys_.begin(); }
const_iterator endFrontals() const { return keys_.begin()+nrFrontals_; } const_iterator endFrontals() const { return Factor::keys_.begin()+nrFrontals_; }
const_iterator beginParents() const { return keys_.begin()+nrFrontals_; } const_iterator beginParents() const { return Factor::keys_.begin()+nrFrontals_; }
const_iterator endParents() const { return keys_.end(); } const_iterator endParents() const { return Factor::keys_.end(); }
/** Mutable iterators and accessors */ /** Mutable iterators and accessors */
iterator beginFrontals() { return keys_.begin(); } iterator beginFrontals() { return Factor::keys_.begin(); }
iterator endFrontals() { return keys_.begin()+nrFrontals_; } iterator endFrontals() { return Factor::keys_.begin()+nrFrontals_; }
iterator beginParents() { return keys_.begin()+nrFrontals_; } iterator beginParents() { return Factor::keys_.begin()+nrFrontals_; }
iterator endParents() { return keys_.end(); } iterator endParents() { return Factor::keys_.end(); }
boost::iterator_range<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); } boost::iterator_range<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
boost::iterator_range<iterator> parents() { return boost::make_iterator_range(beginParents(), endParents()); } boost::iterator_range<iterator> parents() { return boost::make_iterator_range(beginParents(), endParents()); }
@ -134,9 +137,9 @@ public:
/** print */ /** print */
void print(const std::string& s = "Conditional") const { void print(const std::string& s = "Conditional") const {
std::cout << s << " P("; std::cout << s << " P(";
BOOST_FOREACH(Index key, frontals()) std::cout << " " << key; BOOST_FOREACH(Key key, frontals()) std::cout << " " << key;
if (nrParents()>0) std::cout << " |"; if (nrParents()>0) std::cout << " |";
BOOST_FOREACH(Index parent, parents()) std::cout << " " << parent; BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl; std::cout << ")" << std::endl;
} }
@ -146,11 +149,11 @@ public:
*/ */
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG #ifndef NDEBUG
BOOST_FOREACH(Index key, frontals()) { assert(key == inversePermutation[key]); } BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
#endif #endif
bool parentChanged = false; bool parentChanged = false;
BOOST_FOREACH(Index& parent, parents()) { BOOST_FOREACH(Key& parent, parents()) {
Index newParent = inversePermutation[parent]; Key newParent = inversePermutation[parent];
if(parent != newParent) { if(parent != newParent) {
parentChanged = true; parentChanged = true;
parent = newParent; parent = newParent;
@ -166,8 +169,8 @@ public:
void permuteWithInverse(const Permutation& inversePermutation) { void permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals // The permutation may not move the separators into the frontals
#ifndef NDEBUG #ifndef NDEBUG
BOOST_FOREACH(const Index frontal, this->frontals()) { BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Index separator, this->parents()) { BOOST_FOREACH(const Key separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]); assert(inversePermutation[frontal] < inversePermutation[separator]);
} }
} }
@ -181,8 +184,6 @@ protected:
*/ */
void assertInvariants() const; void assertInvariants() const;
friend class Factor;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

34
inference/EliminationTree-inl.h
View File

@ -7,6 +7,7 @@
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/VariableSlots-inl.h> #include <gtsam/inference/VariableSlots-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
@ -19,16 +20,16 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTORGRAPH> template<class FACTOR>
typename EliminationTree<FACTORGRAPH>::EliminationResult typename EliminationTree<FACTOR>::EliminationResult
EliminationTree<FACTORGRAPH>::eliminate_() const { EliminationTree<FACTOR>::eliminate_() const {
typename FACTORGRAPH::bayesnet_type bayesNet; BayesNet bayesNet;
set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > separator; set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > separator;
// Create the list of factors to be eliminated // Create the list of factors to be eliminated
FACTORGRAPH factors; FactorGraph<FACTOR> factors;
factors.reserve(this->factors_.size() + this->subTrees_.size()); factors.reserve(this->factors_.size() + this->subTrees_.size());
// add all factors associated with root // add all factors associated with root
@ -42,15 +43,15 @@ EliminationTree<FACTORGRAPH>::eliminate_() const {
} }
// eliminate the joint factor and add the conditional to the bayes net // eliminate the joint factor and add the conditional to the bayes net
typename FACTORGRAPH::sharedFactor jointFactor(FACTORGRAPH::Factor::Combine(factors, VariableSlots(factors))); sharedFactor jointFactor(FACTOR::Combine(factors, VariableSlots(factors)));
bayesNet.push_back(jointFactor->eliminateFirst()); bayesNet.push_back(jointFactor->eliminateFirst());
return EliminationResult(bayesNet, jointFactor); return EliminationResult(bayesNet, jointFactor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTORGRAPH> template<class FACTOR>
vector<Index> EliminationTree<FACTORGRAPH>::ComputeParents(const VariableIndex<>& structure) { vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex<>& structure) {
// Number of factors and variables // Number of factors and variables
const size_t m = structure.nFactors(); const size_t m = structure.nFactors();
@ -83,9 +84,10 @@ vector<Index> EliminationTree<FACTORGRAPH>::ComputeParents(const VariableIndex<>
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR>
template<class FACTORGRAPH> template<class FACTORGRAPH>
typename EliminationTree<FACTORGRAPH>::shared_ptr typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTORGRAPH>::Create(const FACTORGRAPH& factorGraph) { EliminationTree<FACTOR>::Create(const FACTORGRAPH& factorGraph) {
// Create column structure // Create column structure
VariableIndex<> varIndex(factorGraph); VariableIndex<> varIndex(factorGraph);
@ -108,7 +110,10 @@ EliminationTree<FACTORGRAPH>::Create(const FACTORGRAPH& factorGraph) {
} }
// Hang factors in right places // Hang factors in right places
BOOST_FOREACH(const sharedFactor& factor, factorGraph) { BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& derivedFactor, factorGraph) {
// Here we static_cast to the factor type of this EliminationTree. This
// allows performing symbolic elimination on, for example, GaussianFactors.
sharedFactor factor(boost::shared_static_cast<FACTOR>(derivedFactor));
Index j = factor->front(); Index j = factor->front();
trees[j]->add(factor); trees[j]->add(factor);
} }
@ -148,12 +153,13 @@ bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& ex
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTORGRAPH> template<class FACTOR>
typename FACTORGRAPH::bayesnet_type::shared_ptr EliminationTree<FACTORGRAPH>::eliminate() const { typename EliminationTree<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminate() const {
// call recursive routine // call recursive routine
EliminationResult result = eliminate_(); EliminationResult result = eliminate_();
return typename FACTORGRAPH::bayesnet_type::shared_ptr(new typename FACTORGRAPH::bayesnet_type(result.first)); return typename BayesNet::shared_ptr(new BayesNet(result.first));
} }
} }

19
inference/EliminationTree.h
View File

@ -12,22 +12,24 @@
#include <boost/pool/pool_alloc.hpp> #include <boost/pool/pool_alloc.hpp>
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/BayesNet.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree class EliminationTreeTester; // for unit tests, see testEliminationTree
namespace gtsam { namespace gtsam {
/** /**
* An elimination tree is a tree of factors * An elimination tree is a data structure used intermediately during
* elimination, and it can be used to save work between multiple eliminations.
*/ */
template<class FACTORGRAPH> template<class FACTOR>
class EliminationTree: public Testable<EliminationTree<FACTORGRAPH> > { class EliminationTree: public Testable<EliminationTree<FACTOR> > {
public: public:
typedef boost::shared_ptr<typename FACTORGRAPH::Factor> sharedFactor; typedef typename FACTOR::shared_ptr sharedFactor;
typedef boost::shared_ptr<EliminationTree<FACTORGRAPH> > shared_ptr; typedef boost::shared_ptr<EliminationTree<FACTOR> > shared_ptr;
typedef FACTORGRAPH FactorGraph; typedef gtsam::BayesNet<typename FACTOR::Conditional> BayesNet;
private: private:
@ -38,7 +40,7 @@ private:
Factors factors_; /** factors associated with root */ Factors factors_; /** factors associated with root */
SubTrees subTrees_; /** sub-trees */ SubTrees subTrees_; /** sub-trees */
typedef std::pair<typename FACTORGRAPH::bayesnet_type, typename FACTORGRAPH::sharedFactor> EliminationResult; typedef std::pair<BayesNet, sharedFactor> EliminationResult;
/** 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) {}
@ -66,6 +68,7 @@ private:
public: public:
/** Named constructor to build the elimination tree of a factor graph */ /** Named constructor to build the elimination tree of a factor graph */
template<class FACTORGRAPH>
static shared_ptr Create(const FACTORGRAPH& factorGraph); static shared_ptr Create(const FACTORGRAPH& factorGraph);
/** Print the tree to cout */ /** Print the tree to cout */
@ -75,7 +78,7 @@ public:
bool equals(const EliminationTree& other, double tol = 1e-9) const; bool equals(const EliminationTree& other, double tol = 1e-9) const;
/** Eliminate the factors to a Bayes Net */ /** Eliminate the factors to a Bayes Net */
typename FACTORGRAPH::bayesnet_type::shared_ptr eliminate() const; typename BayesNet::shared_ptr eliminate() const;
}; };
} }

91
inference/Factor-inl.h
View File

@ -25,46 +25,89 @@
#include <boost/iterator/transform_iterator.hpp> #include <boost/iterator/transform_iterator.hpp>
#include <boost/lambda/bind.hpp> #include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
#include <iostream>
namespace gtsam { namespace gtsam {
///* ************************************************************************* */ /* ************************************************************************* */
//template<class ConditionalType> template<typename KEY>
//Factor::Factor(const boost::shared_ptr<ConditionalType>& c) { FactorBase<KEY>::FactorBase(const FactorBase<KEY>& f) : keys_(f.keys_) {}
// keys_.resize(c->parents().size()+1);
// keys_[0] = c->key();
// size_t j = 1;
// BOOST_FOREACH(const Index parent, c->parents()) {
// keys_[j++] = parent;
// }
// checkSorted();
//}
/* ************************************************************************* */ /* ************************************************************************* */
template<class KeyIterator> Factor::Factor(KeyIterator beginKey, KeyIterator endKey) : template<typename KEY>
keys_(beginKey, endKey) { assertInvariants(); } FactorBase<KEY>::FactorBase(const Conditional& c) : keys_(c.keys()) {}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraphType, class VariableIndexStorage> template<typename KEY>
Factor::shared_ptr Factor::Combine(const FactorGraphType& factorGraph, void FactorBase<KEY>::assertInvariants() const {
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors, #ifndef NDEBUG
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) { std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
assert(uniqueSorted.size() == keys_.size());
return shared_ptr(new Factor(variables.begin(), variables.end())); assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class MapAllocator> template<typename KEY>
Factor::shared_ptr Factor::Combine(const FactorGraph<Factor>& factors, const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator>& variableSlots) { void FactorBase<KEY>::print(const std::string& s) const {
typedef const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator> VariableSlots; std::cout << s << " ";
BOOST_FOREACH(KEY key, keys_) std::cout << " " << key;
std::cout << std::endl;
}
/* ************************************************************************* */
template<typename KEY>
//template<class DERIVED>
bool FactorBase<KEY>::equals(const This& other, double tol) const {
return keys_ == other.keys_;
}
/* ************************************************************************* */
template<typename KEY>
template<class DERIVED>
typename DERIVED::shared_ptr FactorBase<KEY>::Combine(const FactorGraph<DERIVED>& factors, const FastMap<Key, std::vector<Key> >& variableSlots) {
typedef const FastMap<Key, std::vector<Key> > VariableSlots;
typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)) FirstGetter; typedef typeof(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)) FirstGetter;
typedef boost::transform_iterator< typedef boost::transform_iterator<
FirstGetter, typename VariableSlots::const_iterator, FirstGetter, typename VariableSlots::const_iterator,
Index, Index> IndexIterator; KEY, KEY> IndexIterator;
FirstGetter firstGetter(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1)); FirstGetter firstGetter(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1));
IndexIterator keysBegin(variableSlots.begin(), firstGetter); IndexIterator keysBegin(variableSlots.begin(), firstGetter);
IndexIterator keysEnd(variableSlots.end(), firstGetter); IndexIterator keysEnd(variableSlots.end(), firstGetter);
return shared_ptr(new Factor(keysBegin, keysEnd)); return typename DERIVED::shared_ptr(new DERIVED(keysBegin, keysEnd));
}
/* ************************************************************************* */
template<typename KEY>
template<class CONDITIONAL>
typename CONDITIONAL::shared_ptr FactorBase<KEY>::eliminateFirst() {
assert(!keys_.empty());
assertInvariants();
KEY eliminated = keys_.front();
keys_.erase(keys_.begin());
return typename CONDITIONAL::shared_ptr(new CONDITIONAL(eliminated, keys_));
}
/* ************************************************************************* */
template<typename KEY>
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr FactorBase<KEY>::eliminate(size_t nrFrontals) {
assert(keys_.size() >= nrFrontals);
assertInvariants();
typename BayesNet<CONDITIONAL>::shared_ptr fragment(new BayesNet<CONDITIONAL>());
const_iterator nextFrontal = this->begin();
for(KEY n = 0; n < nrFrontals; ++n, ++nextFrontal)
fragment->push_back(CONDITIONAL::FromRange(
nextFrontal, const_iterator(this->end()), 1));
if(nrFrontals > 0)
keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents());
return fragment;
}
/* ************************************************************************* */
template<typename KEY>
void FactorBase<KEY>::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(KEY& key, keys_) { key = inversePermutation[key]; }
} }
} }

78
inference/Factor.cpp
View File

@ -1,78 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Factor.cpp
* @brief
* @author Richard Roberts
* @created Sep 1, 2010
*/
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/Conditional.h>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
using namespace std;
using namespace boost::lambda;
namespace gtsam {
/* ************************************************************************* */
Factor::Factor(const Factor& f) : keys_(f.keys_) {}
/* ************************************************************************* */
Factor::Factor(const Conditional& c) : keys_(c.keys()) {}
/* ************************************************************************* */
void Factor::print(const std::string& s) const {
cout << s << " ";
BOOST_FOREACH(Index key, keys_) cout << " " << key;
cout << endl;
}
/* ************************************************************************* */
bool Factor::equals(const Factor& other, double tol) const {
return keys_ == other.keys_;
}
/* ************************************************************************* */
Conditional::shared_ptr Factor::eliminateFirst() {
assert(!keys_.empty());
assertInvariants();
Index eliminated = keys_.front();
keys_.erase(keys_.begin());
return Conditional::shared_ptr(new Conditional(eliminated, keys_));
}
/* ************************************************************************* */
boost::shared_ptr<BayesNet<Conditional> > Factor::eliminate(size_t nrFrontals) {
assert(keys_.size() >= nrFrontals);
assertInvariants();
BayesNet<Conditional>::shared_ptr fragment(new BayesNet<Conditional>());
const_iterator nextFrontal = this->begin();
for(Index n = 0; n < nrFrontals; ++n, ++nextFrontal)
fragment->push_back(Conditional::FromRange(nextFrontal, const_iterator(this->end()), 1));
if(nrFrontals > 0)
keys_.assign(fragment->back()->beginParents(), fragment->back()->endParents());
return fragment;
}
/* ************************************************************************* */
void Factor::permuteWithInverse(const Permutation& inversePermutation) {
BOOST_FOREACH(Index& key, keys_) { key = inversePermutation[key]; }
}
}

81
inference/Factor.h
View File

@ -27,11 +27,12 @@
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
namespace gtsam { namespace gtsam {
class Conditional; template<class KEY> class ConditionalBase;
/** /**
* A simple factor class to use in a factor graph. * A simple factor class to use in a factor graph.
@ -47,10 +48,21 @@ class Conditional;
* variables, continuous ones, or a combination of both. It is up to the config to * variables, continuous ones, or a combination of both. It is up to the config to
* provide the appropriate values at the appropriate time. * provide the appropriate values at the appropriate time.
*/ */
class Factor : public Testable<Factor> { template<typename KEY>
class FactorBase : public Testable<FactorBase<KEY> > {
public:
typedef KEY Key;
typedef FactorBase<Key> This;
typedef gtsam::ConditionalBase<Key> Conditional;
typedef boost::shared_ptr<FactorBase> shared_ptr;
typedef std::vector<Index>::iterator iterator;
typedef std::vector<Index>::const_iterator const_iterator;
protected: protected:
std::vector<Index> keys_; std::vector<Key> keys_;
/** Internal check to make sure keys are sorted. /** Internal check to make sure keys are sorted.
* If NDEBUG is defined, this is empty and optimized out. */ * If NDEBUG is defined, this is empty and optimized out. */
@ -58,61 +70,60 @@ protected:
public: public:
typedef gtsam::Conditional Conditional;
typedef boost::shared_ptr<Factor> shared_ptr;
typedef std::vector<Index>::iterator iterator;
typedef std::vector<Index>::const_iterator const_iterator;
/** Copy constructor */ /** Copy constructor */
Factor(const Factor& f); FactorBase(const This& f);
/** Construct from derived type */ /** Construct from derived type */
Factor(const Conditional& c); FactorBase(const Conditional& c);
/** Constructor from a collection of keys */ /** Constructor from a collection of keys */
template<class KeyIterator> Factor(KeyIterator beginKey, KeyIterator endKey); template<class KeyIterator> FactorBase(KeyIterator beginKey, KeyIterator endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
/** Default constructor for I/O */ /** Default constructor for I/O */
Factor() {} FactorBase() {}
/** Construct unary factor */ /** Construct unary factor */
Factor(Index key) : keys_(1) { FactorBase(Key key) : keys_(1) {
keys_[0] = key; assertInvariants(); } keys_[0] = key; assertInvariants(); }
/** Construct binary factor */ /** Construct binary factor */
Factor(Index key1, Index key2) : keys_(2) { FactorBase(Key key1, Key key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); } keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */ /** Construct ternary factor */
Factor(Index key1, Index key2, Index key3) : keys_(3) { FactorBase(Key key1, Key key2, Key key3) : keys_(3) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); } keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
/** Construct 4-way factor */ /** Construct 4-way factor */
Factor(Index key1, Index key2, Index key3, Index key4) : keys_(4) { FactorBase(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(); }
/** Named constructor for combining a set of factors with pre-computed set of /** Named constructor for combining a set of factors with pre-computed set of
* variables. (Old style - will be removed when scalar elimination is * variables. (Old style - will be removed when scalar elimination is
* removed in favor of the EliminationTree). */ * removed in favor of the EliminationTree). */
template<class FactorGraphType, class VariableIndexStorage> template<class DERIVED, class FactorGraphType, class VariableIndexStorage>
static shared_ptr Combine(const FactorGraphType& factorGraph, static typename DERIVED::shared_ptr Combine(const FactorGraphType& factorGraph,
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors, const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors,
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions); const std::vector<KEY>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
return typename DERIVED::shared_ptr(new DERIVED(variables.begin(), variables.end())); }
/** Create a combined joint factor (new style for EliminationTree). */ /** Create a combined joint factor (new style for EliminationTree). */
template<class MapAllocator> template<class DERIVED>
static shared_ptr Combine(const FactorGraph<Factor>& factors, const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator>& variableSlots); static typename DERIVED::shared_ptr Combine(const FactorGraph<DERIVED>& factors, const FastMap<Key, std::vector<Key> >& variableSlots);
/** /**
* eliminate the first variable involved in this factor * eliminate the first variable involved in this factor
* @return a conditional on the eliminated variable * @return a conditional on the eliminated variable
*/ */
boost::shared_ptr<Conditional> eliminateFirst(); template<class CONDITIONAL>
typename CONDITIONAL::shared_ptr eliminateFirst();
/** /**
* eliminate the first nrFrontals frontal variables. * eliminate the first nrFrontals frontal variables.
*/ */
boost::shared_ptr<BayesNet<Conditional> > eliminate(size_t nrFrontals = 1); template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr eliminate(size_t nrFrontals = 1);
/** /**
* Permutes the GaussianFactor, but for efficiency requires the permutation * Permutes the GaussianFactor, but for efficiency requires the permutation
@ -129,24 +140,25 @@ public:
iterator end() { return keys_.end(); } iterator end() { return keys_.end(); }
/** First key*/ /** First key*/
Index front() const { return keys_.front(); } Key front() const { return keys_.front(); }
/** Last key */ /** Last key */
Index back() const { return keys_.back(); } Key back() const { return keys_.back(); }
/** find */ /** find */
const_iterator find(Index key) const { return std::find(begin(), end(), key); } const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/** print */ /** print */
void print(const std::string& s = "Factor") const; void print(const std::string& s = "Factor") const;
/** check equality */ /** check equality */
bool equals(const Factor& other, double tol = 1e-9) const; // template<class DERIVED>
bool equals(const This& other, double tol = 1e-9) const;
/** /**
* return keys in order as created * return keys in order as created
*/ */
const std::vector<Index>& keys() const { return keys_; } const std::vector<Key>& keys() const { return keys_; }
/** /**
* @return the number of nodes the factor connects * @return the number of nodes the factor connects
@ -155,10 +167,6 @@ public:
protected: protected:
/** Conditional makes internal use of a Factor for storage */
friend class gtsam::Conditional;
friend class GaussianConditional;
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
@ -167,13 +175,4 @@ protected:
} }
}; };
/* ************************************************************************* */
inline void Factor::assertInvariants() const {
#ifndef NDEBUG
std::set<Index> uniqueSorted(keys_.begin(), keys_.end());
assert(uniqueSorted.size() == keys_.size());
assert(std::equal(uniqueSorted.begin(), uniqueSorted.end(), keys_.begin()));
#endif
}
} }

25
inference/IndexConditional.cpp Normal file
View File

@ -0,0 +1,25 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file IndexConditional.cpp
* @brief
* @author Richard Roberts
* @created Oct 17, 2010
*/
#include <gtsam/inference/IndexConditional.h>
namespace gtsam {
template class ConditionalBase<Index>;
}

67
inference/IndexConditional.h Normal file
View File

@ -0,0 +1,67 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file IndexConditional.h
* @brief
* @author Richard Roberts
* @created Oct 17, 2010
*/
#pragma once
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/IndexFactor.h>
namespace gtsam {
class IndexFactor;
class IndexConditional : public ConditionalBase<Index> {
public:
typedef IndexConditional This;
typedef ConditionalBase<Index> Base;
typedef IndexFactor Factor;
typedef boost::shared_ptr<IndexConditional> shared_ptr;
/** Empty Constructor to make serialization possible */
IndexConditional() {}
/** No parents */
IndexConditional(Index j) : Base(j) {}
/** Single parent */
IndexConditional(Index j, Index parent) : Base(j, parent) {}
/** 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) {}
/** 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); }
/** 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); }
};
}

38
inference/IndexFactor.cpp Normal file
View File

@ -0,0 +1,38 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file IndexFactor.cpp
* @brief
* @author Richard Roberts
* @created Oct 17, 2010
*/
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
namespace gtsam {
template class FactorBase<Index>;
IndexFactor::IndexFactor(const IndexConditional& c) : Base(static_cast<const Base>(c)) {}
IndexFactor::shared_ptr IndexFactor::Combine(
const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots) {
return Base::Combine<This>(factors, variableSlots); }
boost::shared_ptr<IndexConditional> IndexFactor::eliminateFirst() {
return Base::eliminateFirst<IndexConditional>(); }
boost::shared_ptr<BayesNet<IndexConditional> > IndexFactor::eliminate(size_t nrFrontals) {
return Base::eliminate<IndexConditional>(nrFrontals); }
}

85
inference/IndexFactor.h Normal file
View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file IndexFactor.h
* @brief
* @author Richard Roberts
* @created Oct 17, 2010
*/
#pragma once
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/Factor.h>
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(static_cast<const 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) {}
/** Create a combined joint factor (new style for EliminationTree). */
static shared_ptr
Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots);
template<class FactorGraphType, class VariableIndexStorage>
static shared_ptr Combine(const FactorGraphType& factorGraph,
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors,
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
return Base::Combine<This>(factorGraph, variableIndex, factors, variables, variablePositions); }
/**
* 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);
};
}

70
inference/IndexTable.h
View File

@ -1,70 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* IndexTable.h
*
* Created on: Jan 21, 2010
* @Author: Frank Dellaert
*/
#pragma once
#include <map>
#include <boost/foreach.hpp> // TODO should not be in header
#include <gtsam/base/Testable.h>
namespace gtsam {
/**
* An IndexTable maps from key to size_t index and back
* most commonly used templated on Symbol with orderings
*/
template<class Key>
class IndexTable: public std::vector<Key>, public Testable<IndexTable<Key> > {
private:
/* map back from key to size_t */
typedef typename std::map<Key, size_t> Map;
Map key2index_;
public:
/* bake ordering into IndexTable */
IndexTable(const std::list<Key>& ordering) {
size_t i = 0;
BOOST_FOREACH(const Key& key,ordering){
this->push_back(key);
key2index_.insert(make_pair(key,i++));
}
}
// Testable
virtual void print(const std::string& s="") const {
std::cout << "IndexTable " << s << ":";
BOOST_FOREACH(Key key,*this) std::cout << (std::string)key << " ";
}
virtual bool equals(const IndexTable<Key>& expected, double tol) const {
return key2index_==expected.key2index_; // TODO, sanity check
}
/** Key to index by parentheses ! */
size_t operator()(const Key& key) const {
typename Map::const_iterator it = key2index_.find(key);
if (it==key2index_.end())
throw(std::invalid_argument("IndexTable::[] invalid key"));
return it->second;
}
/* Index to Key is provided by base class operator[] */
};
}

6
inference/JunctionTree-inl.h
View File

@ -24,6 +24,7 @@
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/VariableSlots-inl.h> #include <gtsam/inference/VariableSlots-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/pool/pool_alloc.hpp> #include <boost/pool/pool_alloc.hpp>
@ -41,7 +42,10 @@ namespace gtsam {
// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph // Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph
// -> SymbolicBayesNet -> SymbolicBayesTree // -> SymbolicBayesNet -> SymbolicBayesTree
tic("JT 1.1 symbolic elimination"); tic("JT 1.1 symbolic elimination");
SymbolicBayesNet::shared_ptr sbn = Inference::EliminateSymbolic(fg); SymbolicBayesNet::shared_ptr sbn = EliminationTree<IndexFactor>::Create(fg)->eliminate();
// SymbolicFactorGraph sfg(fg);
// SymbolicBayesNet::shared_ptr sbn_orig = Inference::Eliminate(sfg);
// assert(assert_equal(*sbn, *sbn_orig));
toc("JT 1.1 symbolic elimination"); toc("JT 1.1 symbolic elimination");
tic("JT 1.2 symbolic BayesTree"); tic("JT 1.2 symbolic BayesTree");
SymbolicBayesTree sbt(*sbn); SymbolicBayesTree sbt(*sbn);

4
inference/JunctionTree.h
View File

@ -26,7 +26,7 @@
#include <boost/pool/pool_alloc.hpp> #include <boost/pool/pool_alloc.hpp>
#include <gtsam/inference/BayesTree.h> #include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/ClusterTree.h> #include <gtsam/inference/ClusterTree.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/IndexConditional.h>
namespace gtsam { namespace gtsam {
@ -49,7 +49,7 @@ namespace gtsam {
typedef class BayesTree<typename FG::Factor::Conditional> BayesTree; typedef class BayesTree<typename FG::Factor::Conditional> BayesTree;
// And we will frequently refer to a symbolic Bayes tree // And we will frequently refer to a symbolic Bayes tree
typedef gtsam::BayesTree<Conditional> SymbolicBayesTree; typedef gtsam::BayesTree<IndexConditional> SymbolicBayesTree;
private: private:
// distribute the factors along the cluster tree // distribute the factors along the cluster tree

5
inference/Makefile.am
View File

@ -15,16 +15,17 @@ check_PROGRAMS =
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# GTSAM core # GTSAM core
headers += SymbolMap.h Factor-inl.h Conditional.h IndexTable.h headers += Factor.h Factor-inl.h Conditional.h
# Symbolic Inference # Symbolic Inference
sources += Factor.cpp SymbolicFactorGraph.cpp sources += SymbolicFactorGraph.cpp
check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional
check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots
# Inference # Inference
headers += inference-inl.h VariableSlots-inl.h headers += inference-inl.h VariableSlots-inl.h
sources += inference.cpp VariableSlots.cpp Permutation.cpp sources += inference.cpp VariableSlots.cpp Permutation.cpp
sources += IndexFactor.cpp IndexConditional.cpp
headers += graph.h graph-inl.h headers += graph.h graph-inl.h
headers += VariableIndex.h headers += VariableIndex.h
headers += FactorGraph.h FactorGraph-inl.h headers += FactorGraph.h FactorGraph-inl.h

172
inference/SymbolMap.h
View File

@ -1,172 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* SymbolMap.h
*
* Created on: Jan 20, 2010
* Author: richard
*/
#pragma once
//#define GTSAM_SYMBOL_HASH
#define GTSAM_SYMBOL_BINARY
#define GTSAM_SYMBOL_SPECIAL
#include <gtsam/nonlinear/Key.h>
#include <map>
#include <boost/unordered_map.hpp>
namespace gtsam {
#ifdef GTSAM_SYMBOL_BINARY
template<class T>
class SymbolMap : public std::map<Symbol, T> {
private:
typedef std::map<Symbol, T> Base;
public:
SymbolMap() : std::map<Symbol, T>() {}
const T& at(const Symbol& key) const {
typename Base::const_iterator it = Base::find(key);
if (it == Base::end())
throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key));
return it->second;
}
T& at(const Symbol& key) {
typename Base::iterator it = Base::find(key);
if (it == Base::end())
throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key));
return it->second;
}
//void find(void);
//void clear() { throw std::runtime_error("Clear should not be used!"); }
};
#endif
#ifdef GTSAM_SYMBOL_HASH
struct SymbolHash : public std::unary_function<Symbol, std::size_t> {
std::size_t operator()(Symbol const& x) const {
std::size_t seed = 0;
boost::hash_combine(seed, x.chr());
boost::hash_combine(seed, x.index());
return ((size_t(x.chr()) << 24) & x.index());
}
};
template<class T>
class SymbolMap : public boost::unordered_map<Symbol, T, SymbolHash> {
public:
SymbolMap() : boost::unordered_map<Symbol, T, SymbolHash>() {}
};
#endif
#ifdef GTSAM_SYMBOL_SPECIAL
template<class T>
class FastSymbolMap {
private:
typedef std::vector<std::vector<T> > Map;
typedef std::vector<T> Vec;
Map values_;
public:
typedef std::pair<Symbol, T> value_type;
FastSymbolMap() {
values_.resize(256);
values_[size_t('x')].reserve(10000);
values_[size_t('l')].reserve(1000);
}
const T& at(const Symbol& key) const {
// typename Map::const_iterator it = values_.find(key.chr());
// if(it != values_.end())
// return it->second.at(key.index());
// else
// throw std::invalid_argument("Key " + (std::string)key + " not present");
return values_.at(size_t(key.chr())).at(key.index());
}
void insert(const value_type& val) {
Vec& vec(values_[size_t(val.first.chr())]);
if(val.first.index() >= vec.size()) {
vec.reserve(val.first.index()+1);
vec.resize(val.first.index());
vec.push_back(val.second);
} else
vec[val.first.index()] = val.second;
}
bool empty() const {
return false;
}
void erase(const Symbol& key) {
}
void clear() {
throw std::runtime_error("Can't clear a FastSymbolMap");
}
// typedef std::pair<Symbol, T> value_type;
//
// class iterator {
// typename Map::iterator map_it_;
// typename Map::iterator map_end_;
// typename Vec::iterator vec_it_;
// public:
// iterator() {}
// iterator(const iterator& it) : map_it_(it.map_it_), vec_it_(it.vec_it_) {}
// bool operator==(const iterator& it);// { return map_it_==it.map_it_ && vec_it_==it.vec_it_; }
// bool operator!=(const iterator& it);// { return map_it_!=it.map_it_ || vec_it_!=it.vec_it_; }
// bool operator*();// { return *it.vec_it_; }
// iterator& operator++(); /* {
// if(map_it_ != map_end_ && vec_it_ == map_it_->second.end())
// do
// vec_it_ = (map_it_++)->second.begin();
// while(map_it_ != map_end_ && vec_it_ == map_it_->second.end());
// else
// vec_it_++;
// return *this;
// }*/
// iterator operator++(int); /* {
// iterator tmp(*this);
// ++(*this);
// return tmp;
// }*/
// };
// class const_iterator {};
// std::size_t size() const;
// T& at(const Symbol& key);
// const_iterator find(const Symbol& key);
// void insert(const std::pair<Symbol, T>& p);
// void clear() { throw std::runtime_error("Clear should not be used!"); }
// std::size_t count() const;
//
// const_iterator begin() const;
// const_iterator end() const;
};
#endif
}

20
inference/SymbolicFactorGraph.cpp
View File

@ -22,7 +22,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/EliminationTree-inl.h> #include <gtsam/inference/EliminationTree-inl.h>
@ -31,35 +31,35 @@ using namespace std;
namespace gtsam { namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere // Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<Factor>; template class FactorGraph<IndexFactor>;
template class BayesNet<Conditional>; template class BayesNet<IndexConditional>;
template class EliminationTree<SymbolicFactorGraph>; template class EliminationTree<IndexFactor>;
/* ************************************************************************* */ /* ************************************************************************* */
SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet<Conditional>& bayesNet) : SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet) :
FactorGraph<Factor>(bayesNet) {} FactorGraph<IndexFactor>(bayesNet) {}
/* ************************************************************************* */ /* ************************************************************************* */
void SymbolicFactorGraph::push_factor(Index key) { void SymbolicFactorGraph::push_factor(Index key) {
boost::shared_ptr<Factor> factor(new Factor(key)); boost::shared_ptr<IndexFactor> factor(new IndexFactor(key));
push_back(factor); push_back(factor);
} }
/** Push back binary factor */ /** Push back binary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2) { void SymbolicFactorGraph::push_factor(Index key1, Index key2) {
boost::shared_ptr<Factor> factor(new Factor(key1,key2)); boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2));
push_back(factor); push_back(factor);
} }
/** Push back ternary factor */ /** Push back ternary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) { void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) {
boost::shared_ptr<Factor> factor(new Factor(key1,key2,key3)); boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2,key3));
push_back(factor); push_back(factor);
} }
/** Push back 4-way factor */ /** Push back 4-way factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) { void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) {
boost::shared_ptr<Factor> factor(new Factor(key1,key2,key3,key4)); boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2,key3,key4));
push_back(factor); push_back(factor);
} }

16
inference/SymbolicFactorGraph.h
View File

@ -22,16 +22,16 @@
#include <list> #include <list>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/IndexConditional.h>
namespace gtsam { namespace gtsam {
typedef BayesNet<Conditional> SymbolicBayesNet; typedef BayesNet<IndexConditional> SymbolicBayesNet;
/** Symbolic Factor Graph */ /** Symbolic IndexFactor Graph */
class SymbolicFactorGraph: public FactorGraph<Factor> { class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
public: public:
typedef SymbolicBayesNet bayesnet_type; typedef SymbolicBayesNet bayesnet_type;
typedef VariableIndex<> variableindex_type; typedef VariableIndex<> variableindex_type;
@ -40,7 +40,7 @@ public:
SymbolicFactorGraph() {} SymbolicFactorGraph() {}
/** Construct from a BayesNet */ /** Construct from a BayesNet */
SymbolicFactorGraph(const BayesNet<Conditional>& bayesNet); SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet);
/** Push back unary factor */ /** Push back unary factor */
void push_factor(Index key); void push_factor(Index key);
@ -77,10 +77,10 @@ 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++) {
if(fg[i]) { if(fg[i]) {
Factor::shared_ptr factor(new Factor(*fg[i])); IndexFactor::shared_ptr factor(new IndexFactor(*fg[i]));
push_back(factor); push_back(factor);
} else } else
push_back(Factor::shared_ptr()); push_back(IndexFactor::shared_ptr());
} }
} }

10
inference/VariableSlots.h
View File

@ -20,11 +20,11 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <map> #include <map>
#include <vector> #include <vector>
#include <string> #include <string>
#include <boost/pool/pool_alloc.hpp>
namespace gtsam { namespace gtsam {
@ -51,15 +51,11 @@ namespace gtsam {
* is not performed by this class. * is not performed by this class.
*/ */
// Internal-use-only typedef for the VariableSlots map base class because this is such a long type name class VariableSlots : public FastMap<Index, std::vector<Index> >, public Testable<VariableSlots> {
typedef std::map<Index, std::vector<Index>, std::less<Index>,
boost::fast_pool_allocator<std::pair<const Index, std::vector<Index> > > > _VariableSlots_map;
class VariableSlots : public _VariableSlots_map, public Testable<VariableSlots> {
public: public:
typedef _VariableSlots_map Base; typedef FastMap<Index, std::vector<Index> > Base;
/** /**
* Constructor from a set of factors to be combined. Sorts the variables * Constructor from a set of factors to be combined. Sorts the variables

42
inference/inference-inl.h
View File

@ -51,25 +51,25 @@ inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(cons
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> //template<class FACTOR>
BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph) { //BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph) {
//
// Create a copy of the factor graph to eliminate in-place // // Create a copy of the factor graph to eliminate in-place
FactorGraph<gtsam::Factor> eliminationGraph(factorGraph); // FactorGraph<gtsam::Factor> eliminationGraph(factorGraph);
VariableIndex<> variableIndex(eliminationGraph); // VariableIndex<> variableIndex(eliminationGraph);
//
typename BayesNet<Conditional>::shared_ptr bayesnet(new BayesNet<Conditional>()); // typename BayesNet<Conditional>::shared_ptr bayesnet(new BayesNet<Conditional>());
//
// Eliminate variables one-by-one, updating the eliminated factor graph and // // Eliminate variables one-by-one, updating the eliminated factor graph and
// the variable index. // // the variable index.
for(Index var = 0; var < variableIndex.size(); ++var) { // for(Index var = 0; var < variableIndex.size(); ++var) {
Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var)); // Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var));
if(conditional) // Will be NULL if the variable did not appear in the factor graph. // if(conditional) // Will be NULL if the variable did not appear in the factor graph.
bayesnet->push_back(conditional); // bayesnet->push_back(conditional);
} // }
//
return bayesnet; // return bayesnet;
} //}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph> template<class FactorGraph>
@ -262,7 +262,9 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
// Join the factors and eliminate the variable from the joint factor // Join the factors and eliminate the variable from the joint factor
tic("EliminateOne: Combine"); tic("EliminateOne: Combine");
typename FactorGraph::sharedFactor jointFactor(FactorGraph::Factor::Combine(factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions)); typename FactorGraph::sharedFactor jointFactor(
FactorGraph::Factor::Combine(
factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions));
toc("EliminateOne: Combine"); toc("EliminateOne: Combine");
// Remove the original factors // Remove the original factors

154
inference/inference.cpp
View File

@ -23,82 +23,82 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Conditional::shared_ptr //Conditional::shared_ptr
Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var) { //Inference::EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var) {
//
tic("EliminateOne"); // tic("EliminateOne");
//
// Get the factors involving the eliminated variable // // Get the factors involving the eliminated variable
VariableIndex<>::mapped_type& varIndexEntry(variableIndex[var]); // VariableIndex<>::mapped_type& varIndexEntry(variableIndex[var]);
typedef VariableIndex<>::mapped_factor_type mapped_factor_type; // typedef VariableIndex<>::mapped_factor_type mapped_factor_type;
//
if(!varIndexEntry.empty()) { // if(!varIndexEntry.empty()) {
//
vector<size_t> removedFactors(varIndexEntry.size()); // vector<size_t> removedFactors(varIndexEntry.size());
transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(), // transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(),
boost::lambda::bind(&VariableIndex<>::mapped_factor_type::factorIndex, boost::lambda::_1)); // boost::lambda::bind(&VariableIndex<>::mapped_factor_type::factorIndex, boost::lambda::_1));
//
// The new joint factor will be the last one in the factor graph // // The new joint factor will be the last one in the factor graph
size_t jointFactorIndex = factorGraph.size(); // size_t jointFactorIndex = factorGraph.size();
//
static const bool debug = false; // static const bool debug = false;
//
if(debug) { // if(debug) {
cout << "Eliminating " << var; // cout << "Eliminating " << var;
factorGraph.print(" from graph: "); // factorGraph.print(" from graph: ");
cout << removedFactors.size() << " factors to remove" << endl; // cout << removedFactors.size() << " factors to remove" << endl;
} // }
//
// Compute the involved keys, uses the variableIndex to mark whether each // // Compute the involved keys, uses the variableIndex to mark whether each
// key has been added yet, but the positions stored in the variableIndex are // // key has been added yet, but the positions stored in the variableIndex are
// from the unsorted positions and will be fixed later. // // from the unsorted positions and will be fixed later.
tic("EliminateOne: Find involved vars"); // tic("EliminateOne: Find involved vars");
typedef set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > InvolvedKeys; // typedef set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > InvolvedKeys;
InvolvedKeys involvedKeys; // InvolvedKeys involvedKeys;
BOOST_FOREACH(size_t removedFactorI, removedFactors) { // BOOST_FOREACH(size_t removedFactorI, removedFactors) {
if(debug) cout << removedFactorI << " is involved" << endl; // if(debug) cout << removedFactorI << " is involved" << endl;
// If the factor has not previously been removed // // If the factor has not previously been removed
if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) { // if(removedFactorI < factorGraph.size() && factorGraph[removedFactorI]) {
// Loop over the variables involved in the removed factor to update the // // Loop over the variables involved in the removed factor to update the
// variable index and joint factor positions of each variable. // // variable index and joint factor positions of each variable.
BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) { // BOOST_FOREACH(Index involvedVariable, factorGraph[removedFactorI]->keys()) {
if(debug) cout << " pulls in variable " << involvedVariable << endl; // if(debug) cout << " pulls in variable " << involvedVariable << endl;
// Mark the new joint factor as involving each variable in the removed factor. // // Mark the new joint factor as involving each variable in the removed factor.
assert(!variableIndex[involvedVariable].empty()); // assert(!variableIndex[involvedVariable].empty());
involvedKeys.insert(involvedVariable); // involvedKeys.insert(involvedVariable);
} // }
} // }
//
// Remove the original factor // // Remove the original factor
factorGraph.remove(removedFactorI); // factorGraph.remove(removedFactorI);
} // }
//
// We need only mark the next variable to be eliminated as involved with the joint factor // // We need only mark the next variable to be eliminated as involved with the joint factor
if(involvedKeys.size() > 1) { // if(involvedKeys.size() > 1) {
InvolvedKeys::const_iterator next = involvedKeys.begin(); ++ next; // InvolvedKeys::const_iterator next = involvedKeys.begin(); ++ next;
variableIndex[*next].push_back(mapped_factor_type(jointFactorIndex,0)); // variableIndex[*next].push_back(mapped_factor_type(jointFactorIndex,0));
} // }
toc("EliminateOne: Find involved vars"); // toc("EliminateOne: Find involved vars");
if(debug) cout << removedFactors.size() << " factors to remove" << endl; // if(debug) cout << removedFactors.size() << " factors to remove" << endl;
//
// Join the factors and eliminate the variable from the joint factor // // Join the factors and eliminate the variable from the joint factor
tic("EliminateOne: Combine"); // tic("EliminateOne: Combine");
Conditional::shared_ptr conditional = Conditional::FromRange(involvedKeys.begin(), involvedKeys.end(), 1); // Conditional::shared_ptr conditional = Conditional::FromRange(involvedKeys.begin(), involvedKeys.end(), 1);
Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents())); // Factor::shared_ptr eliminated(new Factor(conditional->beginParents(), conditional->endParents()));
toc("EliminateOne: Combine"); // toc("EliminateOne: Combine");
//
tic("EliminateOne: store eliminated"); // tic("EliminateOne: store eliminated");
factorGraph.push_back(eliminated); // Put the eliminated factor into the factor graph // factorGraph.push_back(eliminated); // Put the eliminated factor into the factor graph
toc("EliminateOne: store eliminated"); // toc("EliminateOne: store eliminated");
//
toc("EliminateOne"); // toc("EliminateOne");
//
return conditional; // return conditional;
//
} else { // varIndexEntry.empty() // } else { // varIndexEntry.empty()
toc("EliminateOne"); // toc("EliminateOne");
return Conditional::shared_ptr(); // return Conditional::shared_ptr();
} // }
} //}
} }

11
inference/inference.h
View File

@ -29,9 +29,6 @@
namespace gtsam { namespace gtsam {
class Factor;
class Conditional;
class Inference { class Inference {
private: private:
/* Static members only, private constructor */ /* Static members only, private constructor */
@ -51,8 +48,8 @@ class Conditional;
* variables in order starting from 0. Special fast version for symbolic * variables in order starting from 0. Special fast version for symbolic
* elimination. * elimination.
*/ */
template<class FACTOR> // template<class FACTOR>
static BayesNet<Conditional>::shared_ptr EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph); // static BayesNet<Conditional>::shared_ptr EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph);
/** /**
* Eliminate a factor graph in its natural ordering, i.e. eliminating * Eliminate a factor graph in its natural ordering, i.e. eliminating
@ -93,8 +90,8 @@ class Conditional;
* variable index. This is a specialized faster version for purely * variable index. This is a specialized faster version for purely
* symbolic factor graphs. * symbolic factor graphs.
*/ */
static boost::shared_ptr<Conditional> // static boost::shared_ptr<Conditional>
EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var); // EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var);
/** /**
* Eliminate all variables except the specified ones. Internally this * Eliminate all variables except the specified ones. Internally this

84
inference/tests/testBayesTree.cpp
View File

@ -25,22 +25,22 @@ using namespace boost::assign;
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesTree-inl.h> #include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
using namespace gtsam; using namespace gtsam;
typedef BayesTree<Conditional> SymbolicBayesTree; typedef BayesTree<IndexConditional> SymbolicBayesTree;
///* ************************************************************************* */ ///* ************************************************************************* */
//// SLAM example from RSS sqrtSAM paper //// SLAM example from RSS sqrtSAM paper
static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4; static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4;
//Conditional::shared_ptr //IndexConditional::shared_ptr
// x3(new Conditional(_x3_)), // x3(new IndexConditional(_x3_)),
// x2(new Conditional(_x2_,_x3_)), // x2(new IndexConditional(_x2_,_x3_)),
// x1(new Conditional(_x1_,_x2_,_x3_)), // x1(new IndexConditional(_x1_,_x2_,_x3_)),
// l1(new Conditional(_l1_,_x1_,_x2_)), // l1(new IndexConditional(_l1_,_x1_,_x2_)),
// l2(new Conditional(_l2_,_x1_,_x3_)); // l2(new IndexConditional(_l2_,_x1_,_x3_));
// //
//// Bayes Tree for sqrtSAM example //// Bayes Tree for sqrtSAM example
//SymbolicBayesTree createSlamSymbolicBayesTree(){ //SymbolicBayesTree createSlamSymbolicBayesTree(){
@ -58,13 +58,13 @@ static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4;
/* ************************************************************************* */ /* ************************************************************************* */
// Conditionals for ASIA example from the tutorial with A and D evidence // Conditionals for ASIA example from the tutorial with A and D evidence
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
Conditional::shared_ptr IndexConditional::shared_ptr
B(new Conditional(_B_)), B(new IndexConditional(_B_)),
L(new Conditional(_L_, _B_)), L(new IndexConditional(_L_, _B_)),
E(new Conditional(_E_, _L_, _B_)), E(new IndexConditional(_E_, _L_, _B_)),
S(new Conditional(_S_, _L_, _B_)), S(new IndexConditional(_S_, _L_, _B_)),
T(new Conditional(_T_, _E_, _L_)), T(new IndexConditional(_T_, _E_, _L_)),
X(new Conditional(_X_, _E_)); X(new IndexConditional(_X_, _E_));
// Bayes Tree for Asia example // Bayes Tree for Asia example
SymbolicBayesTree createAsiaSymbolicBayesTree() { SymbolicBayesTree createAsiaSymbolicBayesTree() {
@ -89,15 +89,15 @@ TEST( BayesTree, constructor )
LONGS_EQUAL(4,bayesTree.size()); LONGS_EQUAL(4,bayesTree.size());
// Check root // Check root
BayesNet<Conditional> expected_root; BayesNet<IndexConditional> expected_root;
expected_root.push_back(E); expected_root.push_back(E);
expected_root.push_back(L); expected_root.push_back(L);
expected_root.push_back(B); expected_root.push_back(B);
boost::shared_ptr<BayesNet<Conditional> > actual_root = bayesTree.root(); boost::shared_ptr<BayesNet<IndexConditional> > actual_root = bayesTree.root();
CHECK(assert_equal(expected_root,*actual_root)); CHECK(assert_equal(expected_root,*actual_root));
// Create from symbolic Bayes chain in which we want to discover cliques // Create from symbolic Bayes chain in which we want to discover cliques
BayesNet<Conditional> ASIA; BayesNet<IndexConditional> ASIA;
ASIA.push_back(X); ASIA.push_back(X);
ASIA.push_back(T); ASIA.push_back(T);
ASIA.push_back(S); ASIA.push_back(S);
@ -145,13 +145,13 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental.
TEST( BayesTree, removePath ) TEST( BayesTree, removePath )
{ {
const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0; const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
Conditional::shared_ptr IndexConditional::shared_ptr
A(new Conditional(_A_)), A(new IndexConditional(_A_)),
B(new Conditional(_B_, _A_)), B(new IndexConditional(_B_, _A_)),
C(new Conditional(_C_, _A_)), C(new IndexConditional(_C_, _A_)),
D(new Conditional(_D_, _C_)), D(new IndexConditional(_D_, _C_)),
E(new Conditional(_E_, _B_)), E(new IndexConditional(_E_, _B_)),
F(new Conditional(_F_, _E_)); F(new IndexConditional(_F_, _E_));
SymbolicBayesTree bayesTree; SymbolicBayesTree bayesTree;
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; // Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
bayesTree.insert(A); bayesTree.insert(A);
@ -170,7 +170,7 @@ TEST( BayesTree, removePath )
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree[_D_], bayesTree[_E_]; expectedOrphans += bayesTree[_D_], bayesTree[_E_];
BayesNet<Conditional> bn; BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree[_C_], bn, orphans); bayesTree.removePath(bayesTree[_C_], bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraph factors(bn);
@ -183,7 +183,7 @@ TEST( BayesTree, removePath )
SymbolicBayesTree::Cliques expectedOrphans2; SymbolicBayesTree::Cliques expectedOrphans2;
expectedOrphans2 += bayesTree[_F_]; expectedOrphans2 += bayesTree[_F_];
BayesNet<Conditional> bn2; BayesNet<IndexConditional> bn2;
SymbolicBayesTree::Cliques orphans2; SymbolicBayesTree::Cliques orphans2;
bayesTree.removePath(bayesTree[_E_], bn2, orphans2); bayesTree.removePath(bayesTree[_E_], bn2, orphans2);
SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph factors2(bn2);
@ -197,7 +197,7 @@ TEST( BayesTree, removePath2 )
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique B // Call remove-path with clique B
BayesNet<Conditional> bn; BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree[_B_], bn, orphans); bayesTree.removePath(bayesTree[_B_], bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraph factors(bn);
@ -219,7 +219,7 @@ TEST( BayesTree, removePath3 )
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique S // Call remove-path with clique S
BayesNet<Conditional> bn; BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree[_S_], bn, orphans); bayesTree.removePath(bayesTree[_S_], bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraph factors(bn);
@ -242,10 +242,10 @@ TEST( BayesTree, removeTop )
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree(); SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
// create a new factor to be inserted // create a new factor to be inserted
boost::shared_ptr<Factor> newFactor(new Factor(_S_,_B_)); boost::shared_ptr<IndexFactor> newFactor(new IndexFactor(_S_,_B_));
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
BayesNet<Conditional> bn; BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
list<Index> keys; keys += _B_,_S_; list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans); bayesTree.removeTop(keys, bn, orphans);
@ -263,8 +263,8 @@ TEST( BayesTree, removeTop )
CHECK(assert_equal(expectedOrphans, orphans)); CHECK(assert_equal(expectedOrphans, orphans));
// Try removeTop again with a factor that should not change a thing // Try removeTop again with a factor that should not change a thing
boost::shared_ptr<Factor> newFactor2(new Factor(_B_)); boost::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
BayesNet<Conditional> bn2; BayesNet<IndexConditional> bn2;
SymbolicBayesTree::Cliques orphans2; SymbolicBayesTree::Cliques orphans2;
keys.clear(); keys += _B_; keys.clear(); keys += _B_;
bayesTree.removeTop(keys, bn2, orphans2); bayesTree.removeTop(keys, bn2, orphans2);
@ -286,7 +286,7 @@ TEST( BayesTree, removeTop2 )
newFactors.push_factor(_S_); newFactors.push_factor(_S_);
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
BayesNet<Conditional> bn; BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
list<Index> keys; keys += _B_,_S_; list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans); bayesTree.removeTop(keys, bn, orphans);
@ -309,11 +309,11 @@ TEST( BayesTree, removeTop3 )
{ {
const Index _x4_=5, _l5_=6; const Index _x4_=5, _l5_=6;
// simple test case that failed after COLAMD was fixed/activated // simple test case that failed after COLAMD was fixed/activated
Conditional::shared_ptr IndexConditional::shared_ptr
X(new Conditional(_l5_)), X(new IndexConditional(_l5_)),
A(new Conditional(_x4_, _l5_)), A(new IndexConditional(_x4_, _l5_)),
B(new Conditional(_x2_, _x4_)), B(new IndexConditional(_x2_, _x4_)),
C(new Conditional(_x3_, _x2_)); C(new IndexConditional(_x3_, _x2_));
// Ordering newOrdering; // Ordering newOrdering;
// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_; // newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
@ -326,7 +326,7 @@ TEST( BayesTree, removeTop3 )
// remove all // remove all
list<Index> keys; list<Index> keys;
keys += _l5_, _x2_, _x3_, _x4_; keys += _l5_, _x2_, _x3_, _x4_;
BayesNet<Conditional> bn; BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removeTop(keys, bn, orphans); bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraph factors(bn);
@ -356,7 +356,7 @@ TEST( BayesTree, insert )
// Ordering ordering2; ordering2 += _x1_, _x2_; // Ordering ordering2; ordering2 += _x1_, _x2_;
// Ordering ordering3; ordering3 += _x6_, _x5_; // Ordering ordering3; ordering3 += _x6_, _x5_;
BayesNet<Conditional> bn1, bn2, bn3; BayesNet<IndexConditional> bn1, bn2, bn3;
bn1 = *Inference::EliminateUntil(fg1, _x4_+1); bn1 = *Inference::EliminateUntil(fg1, _x4_+1);
bn2 = *Inference::EliminateUntil(fg2, _x2_+1); bn2 = *Inference::EliminateUntil(fg2, _x2_+1);
bn3 = *Inference::EliminateUntil(fg3, _x5_+1); bn3 = *Inference::EliminateUntil(fg3, _x5_+1);
@ -383,7 +383,7 @@ TEST( BayesTree, insert )
fg.push_factor(_x6_, _x4_); fg.push_factor(_x6_, _x4_);
// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_; // Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
BayesNet<Conditional> bn; BayesNet<IndexConditional> bn;
bn = *Inference::Eliminate(fg); bn = *Inference::Eliminate(fg);
SymbolicBayesTree expected(bn); SymbolicBayesTree expected(bn);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));

38
inference/tests/testConditional.cpp
View File

@ -11,7 +11,7 @@
/** /**
* @file testConditional.cpp * @file testConditional.cpp
* @brief Unit tests for Conditional class * @brief Unit tests for IndexConditional class
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -20,76 +20,76 @@
using namespace boost::assign; using namespace boost::assign;
#include <gtsam/CppUnitLite/TestHarness.h> #include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, empty ) TEST( IndexConditional, empty )
{ {
Conditional c0; IndexConditional c0;
LONGS_EQUAL(0,c0.nrFrontals()) LONGS_EQUAL(0,c0.nrFrontals())
LONGS_EQUAL(0,c0.nrParents()) LONGS_EQUAL(0,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, noParents ) TEST( IndexConditional, noParents )
{ {
Conditional c0(0); IndexConditional c0(0);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(0,c0.nrParents()) LONGS_EQUAL(0,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, oneParents ) TEST( IndexConditional, oneParents )
{ {
Conditional c0(0,1); IndexConditional c0(0,1);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(1,c0.nrParents()) LONGS_EQUAL(1,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, twoParents ) TEST( IndexConditional, twoParents )
{ {
Conditional c0(0,1,2); IndexConditional c0(0,1,2);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(2,c0.nrParents()) LONGS_EQUAL(2,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, threeParents ) TEST( IndexConditional, threeParents )
{ {
Conditional c0(0,1,2,3); IndexConditional c0(0,1,2,3);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(3,c0.nrParents()) LONGS_EQUAL(3,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, fourParents ) TEST( IndexConditional, fourParents )
{ {
vector<Index> parents; vector<Index> parents;
parents += 1,2,3,4; parents += 1,2,3,4;
Conditional c0(0,parents); IndexConditional c0(0,parents);
LONGS_EQUAL(1,c0.nrFrontals()) LONGS_EQUAL(1,c0.nrFrontals())
LONGS_EQUAL(4,c0.nrParents()) LONGS_EQUAL(4,c0.nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, FromRange ) TEST( IndexConditional, FromRange )
{ {
list<Index> keys; list<Index> keys;
keys += 1,2,3,4,5; keys += 1,2,3,4,5;
Conditional::shared_ptr c0 = Conditional::FromRange(keys.begin(),keys.end(),2); IndexConditional::shared_ptr c0 = IndexConditional::FromRange(keys.begin(),keys.end(),2);
LONGS_EQUAL(2,c0->nrFrontals()) LONGS_EQUAL(2,c0->nrFrontals())
LONGS_EQUAL(3,c0->nrParents()) LONGS_EQUAL(3,c0->nrParents())
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Conditional, equals ) TEST( IndexConditional, equals )
{ {
Conditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4); IndexConditional c0(0, 1, 2), c1(0, 1, 2), c2(1, 2, 3), c3(3,4);
CHECK(c0.equals(c1)); CHECK(c0.equals(c1));
CHECK(c1.equals(c0)); CHECK(c1.equals(c0));
CHECK(!c0.equals(c2)); CHECK(!c0.equals(c2));

14
inference/tests/testEliminationTree.cpp
View File

@ -8,13 +8,13 @@
#include <gtsam/CppUnitLite/TestHarness.h> #include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
typedef EliminationTree<SymbolicFactorGraph> SymbolicEliminationTree; typedef EliminationTree<IndexFactor> SymbolicEliminationTree;
struct EliminationTreeTester { struct EliminationTreeTester {
// build hardcoded tree // build hardcoded tree
@ -69,11 +69,11 @@ TEST(EliminationTree, Create)
TEST(EliminationTree, eliminate ) TEST(EliminationTree, eliminate )
{ {
// create expected Chordal bayes Net // create expected Chordal bayes Net
Conditional::shared_ptr c0(new Conditional(0, 1, 2)); IndexConditional::shared_ptr c0(new IndexConditional(0, 1, 2));
Conditional::shared_ptr c1(new Conditional(1, 2, 4)); IndexConditional::shared_ptr c1(new IndexConditional(1, 2, 4));
Conditional::shared_ptr c2(new Conditional(2, 4)); IndexConditional::shared_ptr c2(new IndexConditional(2, 4));
Conditional::shared_ptr c3(new Conditional(3, 4)); IndexConditional::shared_ptr c3(new IndexConditional(3, 4));
Conditional::shared_ptr c4(new Conditional(4)); IndexConditional::shared_ptr c4(new IndexConditional(4));
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected.push_back(c3); expected.push_back(c3);

2
inference/tests/testFactorGraph.cpp
View File

@ -11,7 +11,7 @@
/** /**
* @file testFactorgraph.cpp * @file testFactorgraph.cpp
* @brief Unit tests for Factor Graphs * @brief Unit tests for IndexFactor Graphs
* @author Christian Potthast * @author Christian Potthast
**/ **/

4
inference/tests/testISAM.cpp
View File

@ -24,14 +24,14 @@ using namespace boost::assign;
#define GTSAM_MAGIC_KEY #define GTSAM_MAGIC_KEY
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/ISAM-inl.h> #include <gtsam/inference/ISAM-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
typedef ISAM<Conditional> SymbolicISAM; typedef ISAM<IndexConditional> SymbolicISAM;
/* ************************************************************************* */ /* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests // Some numbers that should be consistent among all smoother tests

6
inference/tests/testJunctionTree.cpp
View File

@ -30,12 +30,12 @@ using namespace boost::assign;
#include <gtsam/inference/JunctionTree.h> #include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/ClusterTree-inl.h> #include <gtsam/inference/ClusterTree-inl.h>
#include <gtsam/inference/JunctionTree-inl.h> #include <gtsam/inference/JunctionTree-inl.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
using namespace gtsam; using namespace gtsam;
typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree; typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree;
typedef BayesTree<Conditional> SymbolicBayesTree; typedef BayesTree<IndexConditional> SymbolicBayesTree;
/* ************************************************************************* * /* ************************************************************************* *
* x1 - x2 - x3 - x4 * x1 - x2 - x3 - x4
@ -83,7 +83,7 @@ TEST( JunctionTree, eliminate)
SymbolicJunctionTree jt(fg); SymbolicJunctionTree jt(fg);
SymbolicBayesTree::sharedClique actual = jt.eliminate(); SymbolicBayesTree::sharedClique actual = jt.eliminate();
BayesNet<Conditional> bn = *Inference::Eliminate(fg); BayesNet<IndexConditional> bn = *Inference::Eliminate(fg);
SymbolicBayesTree expected(bn); SymbolicBayesTree expected(bn);
// cout << "BT from JT:\n"; // cout << "BT from JT:\n";

36
inference/tests/testSymbolicBayesNet.cpp
View File

@ -33,17 +33,17 @@ static const Index _A_ = 1;
static const Index _B_ = 2; static const Index _B_ = 2;
static const Index _C_ = 3; static const Index _C_ = 3;
Conditional::shared_ptr IndexConditional::shared_ptr
B(new Conditional(_B_)), B(new IndexConditional(_B_)),
L(new Conditional(_L_, _B_)); L(new IndexConditional(_L_, _B_));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicBayesNet, equals ) TEST( SymbolicBayesNet, equals )
{ {
BayesNet<Conditional> f1; BayesNet<IndexConditional> f1;
f1.push_back(B); f1.push_back(B);
f1.push_back(L); f1.push_back(L);
BayesNet<Conditional> f2; BayesNet<IndexConditional> f2;
f2.push_back(L); f2.push_back(L);
f2.push_back(B); f2.push_back(B);
CHECK(f1.equals(f1)); CHECK(f1.equals(f1));
@ -53,18 +53,18 @@ TEST( SymbolicBayesNet, equals )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicBayesNet, pop_front ) TEST( SymbolicBayesNet, pop_front )
{ {
Conditional::shared_ptr IndexConditional::shared_ptr
A(new Conditional(_A_,_B_,_C_)), A(new IndexConditional(_A_,_B_,_C_)),
B(new Conditional(_B_,_C_)), B(new IndexConditional(_B_,_C_)),
C(new Conditional(_C_)); C(new IndexConditional(_C_));
// Expected after pop_front // Expected after pop_front
BayesNet<Conditional> expected; BayesNet<IndexConditional> expected;
expected.push_back(B); expected.push_back(B);
expected.push_back(C); expected.push_back(C);
// Actual // Actual
BayesNet<Conditional> actual; BayesNet<IndexConditional> actual;
actual.push_back(A); actual.push_back(A);
actual.push_back(B); actual.push_back(B);
actual.push_back(C); actual.push_back(C);
@ -76,24 +76,24 @@ TEST( SymbolicBayesNet, pop_front )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicBayesNet, combine ) TEST( SymbolicBayesNet, combine )
{ {
Conditional::shared_ptr IndexConditional::shared_ptr
A(new Conditional(_A_,_B_,_C_)), A(new IndexConditional(_A_,_B_,_C_)),
B(new Conditional(_B_,_C_)), B(new IndexConditional(_B_,_C_)),
C(new Conditional(_C_)); C(new IndexConditional(_C_));
// p(A|BC) // p(A|BC)
BayesNet<Conditional> p_ABC; BayesNet<IndexConditional> p_ABC;
p_ABC.push_back(A); p_ABC.push_back(A);
// P(BC)=P(B|C)P(C) // P(BC)=P(B|C)P(C)
BayesNet<Conditional> p_BC; BayesNet<IndexConditional> p_BC;
p_BC.push_back(B); p_BC.push_back(B);
p_BC.push_back(C); p_BC.push_back(C);
// P(ABC) = P(A|BC) P(BC) // P(ABC) = P(A|BC) P(BC)
p_ABC.push_back(p_BC); p_ABC.push_back(p_BC);
BayesNet<Conditional> expected; BayesNet<IndexConditional> expected;
expected.push_back(A); expected.push_back(A);
expected.push_back(B); expected.push_back(B);
expected.push_back(C); expected.push_back(C);

20
inference/tests/testSymbolicFactor.cpp
View File

@ -11,14 +11,14 @@
/** /**
* @file testSymbolicFactor.cpp * @file testSymbolicFactor.cpp
* @brief Unit tests for a symbolic Factor * @brief Unit tests for a symbolic IndexFactor
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/CppUnitLite/TestHarness.h> #include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/IndexConditional.h>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
@ -29,17 +29,17 @@ using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicFactor, eliminate) { TEST(SymbolicFactor, eliminate) {
vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
Factor actual(keys.begin(), keys.end()); IndexFactor actual(keys.begin(), keys.end());
BayesNet<Conditional> fragment = *actual.eliminate(3); BayesNet<IndexConditional> fragment = *actual.eliminate(3);
Factor expected(keys.begin()+3, keys.end()); IndexFactor expected(keys.begin()+3, keys.end());
Conditional::shared_ptr expected0 = Conditional::FromRange(keys.begin(), keys.end(), 1); IndexConditional::shared_ptr expected0 = IndexConditional::FromRange(keys.begin(), keys.end(), 1);
Conditional::shared_ptr expected1 = Conditional::FromRange(keys.begin()+1, keys.end(), 1); IndexConditional::shared_ptr expected1 = IndexConditional::FromRange(keys.begin()+1, keys.end(), 1);
Conditional::shared_ptr expected2 = Conditional::FromRange(keys.begin()+2, keys.end(), 1); IndexConditional::shared_ptr expected2 = IndexConditional::FromRange(keys.begin()+2, keys.end(), 1);
CHECK(assert_equal(fragment.size(), size_t(3))); CHECK(assert_equal(fragment.size(), size_t(3)));
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
BayesNet<Conditional>::const_iterator fragmentCond = fragment.begin(); BayesNet<IndexConditional>::const_iterator fragmentCond = fragment.begin();
CHECK(assert_equal(**fragmentCond++, *expected0)); CHECK(assert_equal(**fragmentCond++, *expected0));
CHECK(assert_equal(**fragmentCond++, *expected1)); CHECK(assert_equal(**fragmentCond++, *expected1));
CHECK(assert_equal(**fragmentCond++, *expected2)); CHECK(assert_equal(**fragmentCond++, *expected2));

32
inference/tests/testSymbolicFactorGraph.cpp
View File

@ -11,7 +11,7 @@
/** /**
* @file testSymbolicFactorGraph.cpp * @file testSymbolicFactorGraph.cpp
* @brief Unit tests for a symbolic Factor Graph * @brief Unit tests for a symbolic IndexFactor Graph
* @author Frank Dellaert * @author Frank Dellaert
*/ */
@ -22,7 +22,7 @@ using namespace boost::assign;
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/FactorGraph-inl.h> #include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
@ -43,7 +43,7 @@ TEST( SymbolicFactorGraph, EliminateOne )
VariableIndex<> variableIndex(fg); VariableIndex<> variableIndex(fg);
Inference::EliminateOne(fg, variableIndex, vx2); Inference::EliminateOne(fg, variableIndex, vx2);
SymbolicFactorGraph expected; SymbolicFactorGraph expected;
expected.push_back(boost::shared_ptr<Factor>()); expected.push_back(boost::shared_ptr<IndexFactor>());
expected.push_factor(vx1); expected.push_factor(vx1);
CHECK(assert_equal(expected, fg)); CHECK(assert_equal(expected, fg));
@ -59,11 +59,11 @@ TEST( SymbolicFactorGraph, constructFromBayesNet )
expected.push_factor(vx1); expected.push_factor(vx1);
// create Bayes Net // create Bayes Net
Conditional::shared_ptr x2(new Conditional(vx2, vx1, vl1)); IndexConditional::shared_ptr x2(new IndexConditional(vx2, vx1, vl1));
Conditional::shared_ptr l1(new Conditional(vx1, vl1)); IndexConditional::shared_ptr l1(new IndexConditional(vx1, vl1));
Conditional::shared_ptr x1(new Conditional(vx1)); IndexConditional::shared_ptr x1(new IndexConditional(vx1));
BayesNet<Conditional> bayesNet; BayesNet<IndexConditional> bayesNet;
bayesNet.push_back(x2); bayesNet.push_back(x2);
bayesNet.push_back(l1); bayesNet.push_back(l1);
bayesNet.push_back(x1); bayesNet.push_back(x1);
@ -109,7 +109,7 @@ TEST( SymbolicFactorGraph, push_back )
// //
//public: //public:
// //
// typedef boost::shared_ptr<Factor> sharedFactor; // typedef boost::shared_ptr<IndexFactor> sharedFactor;
// typedef boost::shared_ptr<ETree> shared_ptr; // typedef boost::shared_ptr<ETree> shared_ptr;
// //
//private: //private:
@ -118,7 +118,7 @@ TEST( SymbolicFactorGraph, push_back )
// list<sharedFactor> factors_; /** factors associated with root */ // list<sharedFactor> factors_; /** factors associated with root */
// list<shared_ptr> subTrees_; /** sub-trees */ // list<shared_ptr> subTrees_; /** sub-trees */
// //
// typedef pair<SymbolicBayesNet, Factor> Result; // typedef pair<SymbolicBayesNet, IndexFactor> Result;
// //
// /** // /**
// * Recursive routine that eliminates the factors arranged in an elimination tree // * Recursive routine that eliminates the factors arranged in an elimination tree
@ -146,11 +146,11 @@ TEST( SymbolicFactorGraph, push_back )
// // Make the conditional from the key and separator, and insert it in Bayes net // // Make the conditional from the key and separator, and insert it in Bayes net
// vector<Index> parents; // vector<Index> parents;
// std::copy(separator.begin(), separator.end(), back_inserter(parents)); // std::copy(separator.begin(), separator.end(), back_inserter(parents));
// Conditional::shared_ptr conditional(new Conditional(key_, parents)); // IndexConditional::shared_ptr conditional(new IndexConditional(key_, parents));
// bn.push_back(conditional); // bn.push_back(conditional);
// //
// // now create the new factor from separator to return to caller // // now create the new factor from separator to return to caller
// Factor newFactor(separator.begin(), separator.end()); // IndexFactor newFactor(separator.begin(), separator.end());
// return Result(bn, newFactor); // return Result(bn, newFactor);
// } // }
// //
@ -366,11 +366,11 @@ TEST( SymbolicFactorGraph, push_back )
//TEST( SymbolicFactorGraph, eliminate ) //TEST( SymbolicFactorGraph, eliminate )
//{ //{
// // create expected Chordal bayes Net // // create expected Chordal bayes Net
// Conditional::shared_ptr c0(new Conditional(0, 1, 2)); // IndexConditional::shared_ptr c0(new IndexConditional(0, 1, 2));
// Conditional::shared_ptr c1(new Conditional(1, 2, 4)); // IndexConditional::shared_ptr c1(new IndexConditional(1, 2, 4));
// Conditional::shared_ptr c2(new Conditional(2, 4)); // IndexConditional::shared_ptr c2(new IndexConditional(2, 4));
// Conditional::shared_ptr c3(new Conditional(3, 4)); // IndexConditional::shared_ptr c3(new IndexConditional(3, 4));
// Conditional::shared_ptr c4(new Conditional(4)); // IndexConditional::shared_ptr c4(new IndexConditional(4));
// //
// SymbolicBayesNet expected; // SymbolicBayesNet expected;
// expected.push_back(c3); // expected.push_back(c3);

2
inference/tests/timeSymbolMaps.cpp
View File

@ -23,7 +23,7 @@
#include <vector> #include <vector>
#include <map> #include <map>
#include <gtsam/inference/Key.h> #include <gtsam/nonlinear/Key.h>
using namespace std; using namespace std;
using namespace boost; using namespace boost;

10
linear/GaussianConditional.cpp
View File

@ -33,11 +33,11 @@ namespace gtsam {
GaussianConditional::GaussianConditional() : rsd_(matrix_) {} GaussianConditional::GaussianConditional() : rsd_(matrix_) {}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key) : Conditional(key), rsd_(matrix_) {} GaussianConditional::GaussianConditional(Index key) : IndexConditional(key), rsd_(matrix_) {}
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) : GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix& R, const Vector& sigmas) :
Conditional(key), rsd_(matrix_), sigmas_(sigmas) { IndexConditional(key), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2()); assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), 1 }; size_t dims[] = { R.size2(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size())); rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+2, d.size()));
@ -48,7 +48,7 @@ GaussianConditional::GaussianConditional(Index key,const Vector& d, const Matrix
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, const Vector& sigmas) : Index name1, const Matrix& S, const Vector& sigmas) :
Conditional(key,name1), rsd_(matrix_), sigmas_(sigmas) { IndexConditional(key,name1), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2()); assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), S.size2(), 1 }; size_t dims[] = { R.size2(), S.size2(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size())); rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+3, d.size()));
@ -60,7 +60,7 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri
/* ************************************************************************* */ /* ************************************************************************* */
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R,
Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) : Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas) :
Conditional(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) { IndexConditional(key,name1,name2), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2()); assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), S.size2(), T.size2(), 1 }; size_t dims[] = { R.size2(), S.size2(), T.size2(), 1 };
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+4, d.size())); rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+4, d.size()));
@ -74,7 +74,7 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri
GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) : GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) :
rsd_(matrix_), sigmas_(sigmas) { rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2()); assert(R.size1() <= R.size2());
Conditional::nrFrontals_ = 1; IndexConditional::nrFrontals_ = 1;
keys_.resize(1+parents.size()); keys_.resize(1+parents.size());
size_t dims[1+parents.size()+1]; size_t dims[1+parents.size()+1];
dims[0] = R.size2(); dims[0] = R.size2();

6
linear/GaussianConditional.h
View File

@ -25,7 +25,7 @@
#include <boost/numeric/ublas/triangular.hpp> #include <boost/numeric/ublas/triangular.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h> #include <gtsam/base/blockMatrices.h>
@ -39,10 +39,10 @@ class GaussianFactor;
* It has a set of parents y,z, etc. and implements a probability density on x. * It has a set of parents y,z, etc. and implements a probability density on x.
* The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2 * The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2
*/ */
class GaussianConditional : public Conditional { class GaussianConditional : public IndexConditional {
public: public:
typedef GaussianFactor FactorType; typedef GaussianFactor Factor;
typedef boost::shared_ptr<GaussianConditional> shared_ptr; typedef boost::shared_ptr<GaussianConditional> shared_ptr;
/** Store the conditional matrix as upper-triangular column-major */ /** Store the conditional matrix as upper-triangular column-major */

20
linear/GaussianFactor.cpp
View File

@ -45,14 +45,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
inline void GaussianFactor::assertInvariants() const { inline void GaussianFactor::assertInvariants() const {
#ifndef NDEBUG #ifndef NDEBUG
Factor::assertInvariants(); IndexFactor::assertInvariants();
assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks()); assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks());
#endif #endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::GaussianFactor(const GaussianFactor& gf) : GaussianFactor::GaussianFactor(const GaussianFactor& gf) :
Factor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) { IndexFactor(gf), model_(gf.model_), firstNonzeroBlocks_(gf.firstNonzeroBlocks_), Ab_(matrix_) {
Ab_.assignNoalias(gf.Ab_); Ab_.assignNoalias(gf.Ab_);
assertInvariants(); assertInvariants();
} }
@ -71,7 +71,7 @@ GaussianFactor::GaussianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.si
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, GaussianFactor::GaussianFactor(Index i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) : const Vector& b, const SharedDiagonal& model) :
Factor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { IndexFactor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), 1}; size_t dims[] = { A1.size2(), 1};
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+2, b.size())); Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+2, b.size()));
Ab_(0) = A1; Ab_(0) = A1;
@ -82,7 +82,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1,
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model) : const Vector& b, const SharedDiagonal& model) :
Factor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { IndexFactor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), A2.size2(), 1}; size_t dims[] = { A1.size2(), A2.size2(), 1};
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+3, b.size())); Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+3, b.size()));
Ab_(0) = A1; Ab_(0) = A1;
@ -94,7 +94,7 @@ GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matri
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, GaussianFactor::GaussianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2,
Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) :
Factor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { IndexFactor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) {
size_t dims[] = { A1.size2(), A2.size2(), A3.size2(), 1}; size_t dims[] = { A1.size2(), A2.size2(), A3.size2(), 1};
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+4, b.size())); Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+4, b.size()));
Ab_(0) = A1; Ab_(0) = A1;
@ -147,7 +147,7 @@ GaussianFactor::GaussianFactor(const std::list<std::pair<Index, Matrix> > &terms
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::GaussianFactor(const GaussianConditional& cg) : Factor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) { GaussianFactor::GaussianFactor(const GaussianConditional& cg) : IndexFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) {
Ab_.assignNoalias(cg.rsd_); Ab_.assignNoalias(cg.rsd_);
// todo SL: make firstNonzeroCols triangular? // todo SL: make firstNonzeroCols triangular?
firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions
@ -590,11 +590,11 @@ struct _RowSource {
}; };
/* Explicit instantiations for storage types */ /* Explicit instantiations for storage types */
template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&); template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_vector>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
template GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&); template GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>&, const GaussianVariableIndex<VariableIndexStorage_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
template<class Storage> template<class Storage>
GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& factorGraph, GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<Storage>& variableIndex, const vector<size_t>& factors, const GaussianVariableIndex<Storage>& variableIndex, const vector<size_t>& factors,
const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) { const vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
@ -786,7 +786,7 @@ boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<Gaussia
return boost::make_tuple(varDims, m, n); return boost::make_tuple(varDims, m, n);
} }
GaussianFactor::shared_ptr GaussianFactor::Combine(const GaussianFactorGraph& factors, const VariableSlots& variableSlots) { GaussianFactor::shared_ptr GaussianFactor::Combine(const FactorGraph<GaussianFactor>& factors, const VariableSlots& variableSlots) {
static const bool verbose = false; static const bool verbose = false;
static const bool debug = false; static const bool debug = false;

9
linear/GaussianFactor.h
View File

@ -37,9 +37,10 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/blockMatrices.h> #include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/Factor-inl.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
#include <gtsam/inference/VariableSlots.h> #include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/SharedDiagonal.h> #include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
@ -58,7 +59,7 @@ typedef std::map<Index, size_t> Dimensions;
* GaussianFactor is non-mutable (all methods const!). * GaussianFactor is non-mutable (all methods const!).
* The factor value is exp(-0.5*||Ax-b||^2) * The factor value is exp(-0.5*||Ax-b||^2)
*/ */
class GaussianFactor: public Factor { class GaussianFactor: public IndexFactor {
public: public:
@ -149,7 +150,7 @@ public:
/** Named constructor for combining a set of factors with pre-computed set of /** Named constructor for combining a set of factors with pre-computed set of
* variables. */ * variables. */
template<class Storage> template<class Storage>
static shared_ptr Combine(const GaussianFactorGraph& factorGraph, static shared_ptr Combine(const FactorGraph<GaussianFactor>& factorGraph,
const GaussianVariableIndex<Storage>& variableIndex, const std::vector<size_t>& factors, const GaussianVariableIndex<Storage>& variableIndex, const std::vector<size_t>& factors,
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions); const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions);
@ -157,7 +158,7 @@ public:
* Named constructor for combining a set of factors with pre-computed set of * Named constructor for combining a set of factors with pre-computed set of
* variables. * variables.
*/ */
static shared_ptr Combine(const GaussianFactorGraph& factors, const VariableSlots& variableSlots); static shared_ptr Combine(const FactorGraph<GaussianFactor>& factors, const VariableSlots& variableSlots);
protected: protected:

2
linear/VectorBTree.h
View File

@ -25,7 +25,7 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/inference/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/base/BTree.h> #include <gtsam/base/BTree.h>
namespace gtsam { namespace gtsam {

8
nonlinear/Makefile.am
View File

@ -16,14 +16,16 @@ check_PROGRAMS =
#---------------------------------------------------------------------------------------------------- #----------------------------------------------------------------------------------------------------
# Lie Groups # Lie Groups
headers += Key.h LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h headers += LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h
check_PROGRAMS += tests/testLieValues tests/testKey check_PROGRAMS += tests/testLieValues
# Nonlinear nonlinear # Nonlinear nonlinear
headers += Key.h
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h NonlinearOptimizationParameters.h
headers += NonlinearFactor.h headers += NonlinearFactor.h
sources += NonlinearOptimizer.cpp Ordering.cpp sources += NonlinearOptimizer.cpp Ordering.cpp
check_PROGRAMS += tests/testKey
# Nonlinear constraints # Nonlinear constraints
headers += NonlinearEquality.h headers += NonlinearEquality.h

30
nonlinear/NonlinearFactor.h
View File

@ -26,7 +26,7 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/serialization/base_object.hpp> #include <boost/serialization/base_object.hpp>
#include <gtsam/inference/Factor.h> #include <gtsam/inference/IndexFactor.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/linear/SharedGaussian.h> #include <gtsam/linear/SharedGaussian.h>
@ -138,7 +138,7 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
* variable indices. * variable indices.
*/ */
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const = 0; virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0;
private: private:
@ -204,7 +204,7 @@ namespace gtsam {
Base::print("parent"); Base::print("parent");
} }
/** Check if two factors are equal. Note type is Factor and needs cast. */ /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
bool equals(const NonlinearFactor1<Values,Key>& f, double tol = 1e-9) const { bool equals(const NonlinearFactor1<Values,Key>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_); return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_);
} }
@ -242,8 +242,8 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
* variable indices. * variable indices.
*/ */
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
return Factor::shared_ptr(new Factor(ordering[key_])); return IndexFactor::shared_ptr(new IndexFactor(ordering[key_]));
} }
/* /*
@ -362,12 +362,12 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
* variable indices. * variable indices.
*/ */
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_]; const Index var1 = ordering[key1_], var2 = ordering[key2_];
if(var1 < var2) if(var1 < var2)
return Factor::shared_ptr(new Factor(var1, var2)); return IndexFactor::shared_ptr(new IndexFactor(var1, var2));
else else
return Factor::shared_ptr(new Factor(var2, var1)); return IndexFactor::shared_ptr(new IndexFactor(var2, var1));
} }
/** methods to retrieve both keys */ /** methods to retrieve both keys */
@ -519,20 +519,20 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
* variable indices. * variable indices.
*/ */
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const { virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_]; const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
if(var1 < var2 && var2 < var3) if(var1 < var2 && var2 < var3)
return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key2_], ordering[key3_])); return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_]));
else if(var2 < var1 && var1 < var3) else if(var2 < var1 && var1 < var3)
return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key2_], ordering[key3_])); return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key2_], ordering[key3_]));
else if(var1 < var3 && var3 < var2) else if(var1 < var3 && var3 < var2)
return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key3_], ordering[key2_])); return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key3_], ordering[key2_]));
else if(var2 < var3 && var3 < var1) else if(var2 < var3 && var3 < var1)
return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key3_], ordering[key1_])); return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key3_], ordering[key1_]));
else if(var3 < var1 && var1 < var2) else if(var3 < var1 && var1 < var2)
return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key1_], ordering[key2_])); return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key1_], ordering[key2_]));
else if(var3 < var2 && var2 < var1) else if(var3 < var2 && var2 < var1)
return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key2_], ordering[key1_])); return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key2_], ordering[key1_]));
else else
assert(false); assert(false);
} }

4
nonlinear/NonlinearFactorGraph-inl.h
View File

@ -63,7 +63,7 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
Ordering::shared_ptr NonlinearFactorGraph<Values>::orderingCOLAMD(const Values& config) const { Ordering::shared_ptr NonlinearFactorGraph<Values>::orderingCOLAMD(const Values& config) const {
// Create symbolic graph and initial (iterator) ordering // Create symbolic graph and initial (iterator) ordering
FactorGraph<Factor>::shared_ptr symbolic; SymbolicFactorGraph::shared_ptr symbolic;
Ordering::shared_ptr ordering; Ordering::shared_ptr ordering;
boost::tie(symbolic,ordering) = this->symbolic(config); boost::tie(symbolic,ordering) = this->symbolic(config);
@ -88,7 +88,7 @@ void NonlinearFactorGraph<Values>::print(const std::string& str) const {
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<Values>::symbolic( SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<Values>::symbolic(
const Values& config, const Ordering& ordering) const { const Values& config, const Ordering& ordering) const {
// Generate the symbolic factor graph // Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new FactorGraph<Factor>); SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size()); symbolicfg->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
symbolicfg->push_back(factor->symbolic(ordering)); symbolicfg->push_back(factor->symbolic(ordering));

2
slam/saveGraph.h
View File

@ -21,7 +21,7 @@
#include <gtsam/inference/FactorGraph.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/SymbolicFactor.h> #include <gtsam/inference/SymbolicFactor.h>
#include <gtsam/inference/SymbolicBayesNet.h> #include <gtsam/inference/SymbolicBayesNet.h>
#include <gtsam/inference/Key.h> #include <gtsam/nonlinear/Key.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/LieValues.h> #include <gtsam/nonlinear/LieValues.h>

8
tests/testGaussianFactorGraph.cpp
View File

@ -314,7 +314,7 @@ TEST( GaussianFactorGraph, eliminateAll )
GaussianBayesNet actual = *Inference::Eliminate(fg1); GaussianBayesNet actual = *Inference::Eliminate(fg1);
CHECK(assert_equal(expected,actual,tol)); CHECK(assert_equal(expected,actual,tol));
GaussianBayesNet actualET = *EliminationTree<GaussianFactorGraph>::Create(fg1)->eliminate(); GaussianBayesNet actualET = *EliminationTree<GaussianFactor>::Create(fg1)->eliminate();
CHECK(assert_equal(expected,actualET,tol)); CHECK(assert_equal(expected,actualET,tol));
} }
@ -463,7 +463,7 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
TEST( GaussianFactorGraph, getOrdering) TEST( GaussianFactorGraph, getOrdering)
{ {
Ordering original; original += "l1","x1","x2"; Ordering original; original += "l1","x1","x2";
FactorGraph<Factor> symbolic(createGaussianFactorGraph(original)); FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
Permutation perm(*Inference::PermutationCOLAMD(VariableIndex<>(symbolic))); Permutation perm(*Inference::PermutationCOLAMD(VariableIndex<>(symbolic)));
Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
Ordering expected; expected += "l1","x2","x1"; Ordering expected; expected += "l1","x2","x1";
@ -491,7 +491,7 @@ TEST( GaussianFactorGraph, optimize )
// optimize the graph // optimize the graph
VectorValues actual = optimize(*Inference::Eliminate(fg)); VectorValues actual = optimize(*Inference::Eliminate(fg));
VectorValues actualET = optimize(*EliminationTree<GaussianFactorGraph>::Create(fg)->eliminate()); VectorValues actualET = optimize(*EliminationTree<GaussianFactor>::Create(fg)->eliminate());
// verify // verify
VectorValues expected = createCorrectDelta(ord); VectorValues expected = createCorrectDelta(ord);
@ -786,7 +786,7 @@ TEST( GaussianFactorGraph, constrained_simple )
// eliminate and solve // eliminate and solve
VectorValues actual = optimize(*Inference::Eliminate(fg)); VectorValues actual = optimize(*Inference::Eliminate(fg));
VectorValues actualET = optimize(*EliminationTree<GaussianFactorGraph>::Create(fg)->eliminate()); VectorValues actualET = optimize(*EliminationTree<GaussianFactor>::Create(fg)->eliminate());
// verify // verify
VectorValues expected = createSimpleConstraintValues(); VectorValues expected = createSimpleConstraintValues();

18
tests/testGaussianJunctionTree.cpp
View File

@ -69,15 +69,15 @@ TEST( GaussianJunctionTree, constructor2 )
list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it; list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
GaussianJunctionTree::sharedClique child0 = *child0it; GaussianJunctionTree::sharedClique child0 = *child0it;
GaussianJunctionTree::sharedClique child1 = *child1it; GaussianJunctionTree::sharedClique child1 = *child1it;
CHECK(assert_equal(frontal2, child0->frontal)); CHECK(assert_equal(frontal2, child1->frontal));
CHECK(assert_equal(sep2, child0->separator)); CHECK(assert_equal(sep2, child1->separator));
LONGS_EQUAL(4, child0->size()); LONGS_EQUAL(4, child1->size());
CHECK(assert_equal(frontal3, child0->children().front()->frontal)); CHECK(assert_equal(frontal3, child1->children().front()->frontal));
CHECK(assert_equal(sep3, child0->children().front()->separator)); CHECK(assert_equal(sep3, child1->children().front()->separator));
LONGS_EQUAL(2, child0->children().front()->size()); LONGS_EQUAL(2, child1->children().front()->size());
CHECK(assert_equal(frontal4, child1->frontal)); CHECK(assert_equal(frontal4, child0->frontal));
CHECK(assert_equal(sep4, child1->separator)); CHECK(assert_equal(sep4, child0->separator));
LONGS_EQUAL(2, child1->size()); LONGS_EQUAL(2, child0->size());
} }
/* ************************************************************************* */ /* ************************************************************************* */

17
tests/testSymbolicBayesNet.cpp
View File

@ -26,7 +26,6 @@ using namespace boost::assign;
#include <gtsam/slam/smallExample.h> #include <gtsam/slam/smallExample.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
using namespace std; using namespace std;
@ -34,20 +33,20 @@ using namespace gtsam;
using namespace example; using namespace example;
//Symbol _B_('B', 0), _L_('L', 0); //Symbol _B_('B', 0), _L_('L', 0);
//Conditional::shared_ptr //IndexConditional::shared_ptr
// B(new Conditional(_B_)), // B(new IndexConditional(_B_)),
// L(new Conditional(_L_, _B_)); // L(new IndexConditional(_L_, _B_));
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicBayesNet, constructor ) TEST( SymbolicBayesNet, constructor )
{ {
Ordering o; o += "x2","l1","x1"; Ordering o; o += "x2","l1","x1";
// Create manually // Create manually
Conditional::shared_ptr IndexConditional::shared_ptr
x2(new Conditional(o["x2"],o["l1"], o["x1"])), x2(new IndexConditional(o["x2"],o["l1"], o["x1"])),
l1(new Conditional(o["l1"],o["x1"])), l1(new IndexConditional(o["l1"],o["x1"])),
x1(new Conditional(o["x1"])); x1(new IndexConditional(o["x1"]));
BayesNet<Conditional> expected; BayesNet<IndexConditional> expected;
expected.push_back(x2); expected.push_back(x2);
expected.push_back(l1); expected.push_back(l1);
expected.push_back(x1); expected.push_back(x1);

13
tests/testSymbolicFactorGraph.cpp
View File

@ -24,7 +24,6 @@ using namespace boost::assign;
#include <gtsam/slam/smallExample.h> #include <gtsam/slam/smallExample.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/BayesNet-inl.h> #include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/FactorGraph-inl.h> #include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/inference-inl.h> #include <gtsam/inference/inference-inl.h>
@ -117,10 +116,10 @@ TEST( SymbolicFactorGraph, eliminateOne )
// eliminate // eliminate
VariableIndex<> varindex(fg); VariableIndex<> varindex(fg);
Conditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, o["x1"]); IndexConditional::shared_ptr actual = Inference::EliminateOne(fg, varindex, o["x1"]);
// create expected symbolic Conditional // create expected symbolic IndexConditional
Conditional expected(o["x1"],o["l1"],o["x2"]); IndexConditional expected(o["x1"],o["l1"],o["x2"]);
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
} }
@ -131,9 +130,9 @@ TEST( SymbolicFactorGraph, eliminate )
Ordering o; o += "x2","l1","x1"; Ordering o; o += "x2","l1","x1";
// create expected Chordal bayes Net // create expected Chordal bayes Net
Conditional::shared_ptr x2(new Conditional(o["x2"], o["l1"], o["x1"])); IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
Conditional::shared_ptr l1(new Conditional(o["l1"], o["x1"])); IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
Conditional::shared_ptr x1(new Conditional(o["x1"])); IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected.push_back(x2); expected.push_back(x2);