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
* 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:
@ -47,40 +48,41 @@ protected:
public:
/** convenience typename for a shared pointer to this class */
typedef gtsam::Factor Factor;
typedef boost::shared_ptr<Conditional> shared_ptr;
typedef Factor::iterator iterator;
typedef Factor::const_iterator const_iterator;
typedef KEY Key;
typedef ConditionalBase<Key> This;
typedef gtsam::FactorBase<Key> Factor;
typedef boost::shared_ptr<This> shared_ptr;
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> Parents;
/** Empty Constructor to make serialization possible */
Conditional() : nrFrontals_(0) {}
ConditionalBase() : nrFrontals_(0) {}
/** No parents */
Conditional(Index key) : Factor(key), nrFrontals_(1) {}
ConditionalBase(Key key) : Factor(key), nrFrontals_(1) {}
/** Single parent */
Conditional(Index key, Index parent) : Factor(key, parent), nrFrontals_(1) {}
ConditionalBase(Key key, Key parent) : Factor(key, parent), nrFrontals_(1) {}
/** 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 */
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 */
Conditional(Index key, const std::vector<Index>& parents) : nrFrontals_(1) {
keys_.resize(1 + parents.size());
ConditionalBase(Key key, const std::vector<Key>& parents) : nrFrontals_(1) {
Factor::keys_.resize(1 + parents.size());
*(beginFrontals()) = key;
std::copy(parents.begin(), parents.end(), beginParents());
}
/** Constructor from a frontal variable and an iterator range of parents */
template<typename Iterator>
static Conditional::shared_ptr FromRange(Index key, Iterator firstParent, Iterator lastParent) {
Conditional::shared_ptr conditional(new Conditional);
template<class DERIVED, typename ITERATOR>
static typename DERIVED::shared_ptr FromRange(Key key, ITERATOR firstParent, ITERATOR lastParent) {
typename DERIVED::shared_ptr conditional(new DERIVED);
conditional->nrFrontals_ = 1;
conditional->keys_.push_back(key);
std::copy(firstParent, lastParent, back_inserter(conditional->keys_));
@ -88,38 +90,39 @@ public:
}
/** Named constructor from any number of frontal variables and parents */
template<typename Iterator>
static Conditional::shared_ptr FromRange(Iterator firstKey, Iterator lastKey, size_t nrFrontals) {
Conditional::shared_ptr conditional(new Conditional);
template<typename DERIVED, typename ITERATOR>
static typename DERIVED::shared_ptr FromRange(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) {
typename DERIVED::shared_ptr conditional(new DERIVED);
conditional->nrFrontals_ = nrFrontals;
std::copy(firstKey, lastKey, back_inserter(conditional->keys_));
return conditional;
}
/** 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 the number of frontals */
size_t nrFrontals() const { return nrFrontals_; }
/** 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. */
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. */
const_iterator beginFrontals() const { return keys_.begin(); }
const_iterator endFrontals() const { return keys_.begin()+nrFrontals_; }
const_iterator beginParents() const { return keys_.begin()+nrFrontals_; }
const_iterator endParents() const { return keys_.end(); }
const_iterator beginFrontals() const { return Factor::keys_.begin(); }
const_iterator endFrontals() const { return Factor::keys_.begin()+nrFrontals_; }
const_iterator beginParents() const { return Factor::keys_.begin()+nrFrontals_; }
const_iterator endParents() const { return Factor::keys_.end(); }
/** Mutable iterators and accessors */
iterator beginFrontals() { return keys_.begin(); }
iterator endFrontals() { return keys_.begin()+nrFrontals_; }
iterator beginParents() { return keys_.begin()+nrFrontals_; }
iterator endParents() { return keys_.end(); }
iterator beginFrontals() { return Factor::keys_.begin(); }
iterator endFrontals() { return Factor::keys_.begin()+nrFrontals_; }
iterator beginParents() { return Factor::keys_.begin()+nrFrontals_; }
iterator endParents() { return Factor::keys_.end(); }
boost::iterator_range<iterator> frontals() { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
boost::iterator_range<iterator> parents() { return boost::make_iterator_range(beginParents(), endParents()); }
@ -134,9 +137,9 @@ public:
/** print */
void print(const std::string& s = "Conditional") const {
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 << " |";
BOOST_FOREACH(Index parent, parents()) std::cout << " " << parent;
BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl;
}
@ -146,11 +149,11 @@ public:
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(Index key, frontals()) { assert(key == inversePermutation[key]); }
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(Index& parent, parents()) {
Index newParent = inversePermutation[parent];
BOOST_FOREACH(Key& parent, parents()) {
Key newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
@ -166,8 +169,8 @@ public:
void permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const Index frontal, this->frontals()) {
BOOST_FOREACH(const Index separator, this->parents()) {
BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}
@ -181,8 +184,6 @@ protected:
*/
void assertInvariants() const;
friend class Factor;
private:
/** Serialization function */
friend class boost::serialization::access;

34
inference/EliminationTree-inl.h
View File

@ -7,6 +7,7 @@
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/VariableSlots-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <boost/foreach.hpp>
#include <boost/lambda/lambda.hpp>
@ -19,16 +20,16 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class FACTORGRAPH>
typename EliminationTree<FACTORGRAPH>::EliminationResult
EliminationTree<FACTORGRAPH>::eliminate_() const {
template<class FACTOR>
typename EliminationTree<FACTOR>::EliminationResult
EliminationTree<FACTOR>::eliminate_() const {
typename FACTORGRAPH::bayesnet_type bayesNet;
BayesNet bayesNet;
set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > separator;
// Create the list of factors to be eliminated
FACTORGRAPH factors;
FactorGraph<FACTOR> factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// 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
typename FACTORGRAPH::sharedFactor jointFactor(FACTORGRAPH::Factor::Combine(factors, VariableSlots(factors)));
sharedFactor jointFactor(FACTOR::Combine(factors, VariableSlots(factors)));
bayesNet.push_back(jointFactor->eliminateFirst());
return EliminationResult(bayesNet, jointFactor);
}
/* ************************************************************************* */
template<class FACTORGRAPH>
vector<Index> EliminationTree<FACTORGRAPH>::ComputeParents(const VariableIndex<>& structure) {
template<class FACTOR>
vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex<>& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
@ -83,9 +84,10 @@ vector<Index> EliminationTree<FACTORGRAPH>::ComputeParents(const VariableIndex<>
}
/* ************************************************************************* */
template<class FACTOR>
template<class FACTORGRAPH>
typename EliminationTree<FACTORGRAPH>::shared_ptr
EliminationTree<FACTORGRAPH>::Create(const FACTORGRAPH& factorGraph) {
typename EliminationTree<FACTOR>::shared_ptr
EliminationTree<FACTOR>::Create(const FACTORGRAPH& factorGraph) {
// Create column structure
VariableIndex<> varIndex(factorGraph);
@ -108,7 +110,10 @@ EliminationTree<FACTORGRAPH>::Create(const FACTORGRAPH& factorGraph) {
}
// 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();
trees[j]->add(factor);
}
@ -148,12 +153,13 @@ bool EliminationTree<FACTORGRAPH>::equals(const EliminationTree<FACTORGRAPH>& ex
}
/* ************************************************************************* */
template<class FACTORGRAPH>
typename FACTORGRAPH::bayesnet_type::shared_ptr EliminationTree<FACTORGRAPH>::eliminate() const {
template<class FACTOR>
typename EliminationTree<FACTOR>::BayesNet::shared_ptr
EliminationTree<FACTOR>::eliminate() const {
// call recursive routine
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 <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/BayesNet.h>
class EliminationTreeTester; // for unit tests, see testEliminationTree
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>
class EliminationTree: public Testable<EliminationTree<FACTORGRAPH> > {
template<class FACTOR>
class EliminationTree: public Testable<EliminationTree<FACTOR> > {
public:
typedef boost::shared_ptr<typename FACTORGRAPH::Factor> sharedFactor;
typedef boost::shared_ptr<EliminationTree<FACTORGRAPH> > shared_ptr;
typedef FACTORGRAPH FactorGraph;
typedef typename FACTOR::shared_ptr sharedFactor;
typedef boost::shared_ptr<EliminationTree<FACTOR> > shared_ptr;
typedef gtsam::BayesNet<typename FACTOR::Conditional> BayesNet;
private:
@ -38,7 +40,7 @@ private:
Factors factors_; /** factors associated with root */
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 */
EliminationTree(Index key = 0) : key_(key) {}
@ -66,6 +68,7 @@ private:
public:
/** Named constructor to build the elimination tree of a factor graph */
template<class FACTORGRAPH>
static shared_ptr Create(const FACTORGRAPH& factorGraph);
/** Print the tree to cout */
@ -75,7 +78,7 @@ public:
bool equals(const EliminationTree& other, double tol = 1e-9) const;
/** 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/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include <iostream>
namespace gtsam {
///* ************************************************************************* */
//template<class ConditionalType>
//Factor::Factor(const boost::shared_ptr<ConditionalType>& c) {
// 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<typename KEY>
FactorBase<KEY>::FactorBase(const FactorBase<KEY>& f) : keys_(f.keys_) {}
/* ************************************************************************* */
template<class KeyIterator> Factor::Factor(KeyIterator beginKey, KeyIterator endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
template<typename KEY>
FactorBase<KEY>::FactorBase(const Conditional& c) : keys_(c.keys()) {}
/* ************************************************************************* */
template<class FactorGraphType, class VariableIndexStorage>
Factor::shared_ptr Factor::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 shared_ptr(new Factor(variables.begin(), variables.end()));
template<typename KEY>
void FactorBase<KEY>::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
}
/* ************************************************************************* */
template<class MapAllocator>
Factor::shared_ptr Factor::Combine(const FactorGraph<Factor>& factors, const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator>& variableSlots) {
typedef const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator> VariableSlots;
template<typename KEY>
void FactorBase<KEY>::print(const std::string& s) const {
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 boost::transform_iterator<
FirstGetter, typename VariableSlots::const_iterator,
Index, Index> IndexIterator;
KEY, KEY> IndexIterator;
FirstGetter firstGetter(boost::lambda::bind(&VariableSlots::value_type::first, boost::lambda::_1));
IndexIterator keysBegin(variableSlots.begin(), 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 <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/inference.h>
namespace gtsam {
class Conditional;
template<class KEY> class ConditionalBase;
/**
* 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
* 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:
std::vector<Index> keys_;
std::vector<Key> keys_;
/** Internal check to make sure keys are sorted.
* If NDEBUG is defined, this is empty and optimized out. */
@ -58,61 +70,60 @@ protected:
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 */
Factor(const Factor& f);
FactorBase(const This& f);
/** Construct from derived type */
Factor(const Conditional& c);
FactorBase(const Conditional& c);
/** 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 */
Factor() {}
FactorBase() {}
/** Construct unary factor */
Factor(Index key) : keys_(1) {
FactorBase(Key key) : keys_(1) {
keys_[0] = key; assertInvariants(); }
/** Construct binary factor */
Factor(Index key1, Index key2) : keys_(2) {
FactorBase(Key key1, Key key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** 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(); }
/** 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(); }
/** Named constructor for combining a set of factors with pre-computed set of
* variables. (Old style - will be removed when scalar elimination is
* removed in favor of the EliminationTree). */
template<class FactorGraphType, class VariableIndexStorage>
static shared_ptr Combine(const FactorGraphType& factorGraph,
template<class DERIVED, class FactorGraphType, class VariableIndexStorage>
static typename DERIVED::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);
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). */
template<class MapAllocator>
static shared_ptr Combine(const FactorGraph<Factor>& factors, const std::map<Index, std::vector<Index>, std::less<Index>, MapAllocator>& variableSlots);
template<class DERIVED>
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
* @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.
*/
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
@ -129,24 +140,25 @@ public:
iterator end() { return keys_.end(); }
/** First key*/
Index front() const { return keys_.front(); }
Key front() const { return keys_.front(); }
/** Last key */
Index back() const { return keys_.back(); }
Key back() const { return keys_.back(); }
/** 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 */
void print(const std::string& s = "Factor") const;
/** 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
*/
const std::vector<Index>& keys() const { return keys_; }
const std::vector<Key>& keys() const { return keys_; }
/**
* @return the number of nodes the factor connects
@ -155,10 +167,6 @@ public:
protected:
/** Conditional makes internal use of a Factor for storage */
friend class gtsam::Conditional;
friend class GaussianConditional;
/** Serialization function */
friend class boost::serialization::access;
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/inference-inl.h>
#include <gtsam/inference/VariableSlots-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <boost/foreach.hpp>
#include <boost/pool/pool_alloc.hpp>
@ -41,7 +42,10 @@ namespace gtsam {
// Symbolic factorization: GaussianFactorGraph -> SymbolicFactorGraph
// -> SymbolicBayesNet -> SymbolicBayesTree
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");
tic("JT 1.2 symbolic BayesTree");
SymbolicBayesTree sbt(*sbn);

4
inference/JunctionTree.h
View File

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

5
inference/Makefile.am
View File

@ -15,16 +15,17 @@ check_PROGRAMS =
#----------------------------------------------------------------------------------------------------
# GTSAM core
headers += SymbolMap.h Factor-inl.h Conditional.h IndexTable.h
headers += Factor.h Factor-inl.h Conditional.h
# Symbolic Inference
sources += Factor.cpp SymbolicFactorGraph.cpp
sources += SymbolicFactorGraph.cpp
check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional
check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots
# Inference
headers += inference-inl.h VariableSlots-inl.h
sources += inference.cpp VariableSlots.cpp Permutation.cpp
sources += IndexFactor.cpp IndexConditional.cpp
headers += graph.h graph-inl.h
headers += VariableIndex.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 <gtsam/inference/SymbolicFactorGraph.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/EliminationTree-inl.h>
@ -31,35 +31,35 @@ using namespace std;
namespace gtsam {
// Explicitly instantiate so we don't have to include everywhere
template class FactorGraph<Factor>;
template class BayesNet<Conditional>;
template class EliminationTree<SymbolicFactorGraph>;
template class FactorGraph<IndexFactor>;
template class BayesNet<IndexConditional>;
template class EliminationTree<IndexFactor>;
/* ************************************************************************* */
SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet<Conditional>& bayesNet) :
FactorGraph<Factor>(bayesNet) {}
SymbolicFactorGraph::SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet) :
FactorGraph<IndexFactor>(bayesNet) {}
/* ************************************************************************* */
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 binary factor */
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 ternary factor */
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 4-way factor */
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);
}

16
inference/SymbolicFactorGraph.h
View File

@ -22,16 +22,16 @@
#include <list>
#include <gtsam/base/types.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/Conditional.h>
#include <gtsam/inference/IndexConditional.h>
namespace gtsam {
typedef BayesNet<Conditional> SymbolicBayesNet;
typedef BayesNet<IndexConditional> SymbolicBayesNet;
/** Symbolic Factor Graph */
class SymbolicFactorGraph: public FactorGraph<Factor> {
/** Symbolic IndexFactor Graph */
class SymbolicFactorGraph: public FactorGraph<IndexFactor> {
public:
typedef SymbolicBayesNet bayesnet_type;
typedef VariableIndex<> variableindex_type;
@ -40,7 +40,7 @@ public:
SymbolicFactorGraph() {}
/** Construct from a BayesNet */
SymbolicFactorGraph(const BayesNet<Conditional>& bayesNet);
SymbolicFactorGraph(const BayesNet<IndexConditional>& bayesNet);
/** Push back unary factor */
void push_factor(Index key);
@ -77,10 +77,10 @@ template<class FACTOR>
SymbolicFactorGraph::SymbolicFactorGraph(const FactorGraph<FACTOR>& fg) {
for (size_t i = 0; i < fg.size(); i++) {
if(fg[i]) {
Factor::shared_ptr factor(new Factor(*fg[i]));
IndexFactor::shared_ptr factor(new IndexFactor(*fg[i]));
push_back(factor);
} 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/Testable.h>
#include <gtsam/base/FastMap.h>
#include <map>
#include <vector>
#include <string>
#include <boost/pool/pool_alloc.hpp>
namespace gtsam {
@ -51,15 +51,11 @@ namespace gtsam {
* is not performed by this class.
*/
// Internal-use-only typedef for the VariableSlots map base class because this is such a long type name
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> {
class VariableSlots : public FastMap<Index, std::vector<Index> >, public Testable<VariableSlots> {
public:
typedef _VariableSlots_map Base;
typedef FastMap<Index, std::vector<Index> > Base;
/**
* 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>
BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph) {
// Create a copy of the factor graph to eliminate in-place
FactorGraph<gtsam::Factor> eliminationGraph(factorGraph);
VariableIndex<> variableIndex(eliminationGraph);
typename BayesNet<Conditional>::shared_ptr bayesnet(new BayesNet<Conditional>());
// Eliminate variables one-by-one, updating the eliminated factor graph and
// the variable index.
for(Index var = 0; var < variableIndex.size(); ++var) {
Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var));
if(conditional) // Will be NULL if the variable did not appear in the factor graph.
bayesnet->push_back(conditional);
}
return bayesnet;
}
//template<class FACTOR>
//BayesNet<Conditional>::shared_ptr Inference::EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph) {
//
// // Create a copy of the factor graph to eliminate in-place
// FactorGraph<gtsam::Factor> eliminationGraph(factorGraph);
// VariableIndex<> variableIndex(eliminationGraph);
//
// typename BayesNet<Conditional>::shared_ptr bayesnet(new BayesNet<Conditional>());
//
// // Eliminate variables one-by-one, updating the eliminated factor graph and
// // the variable index.
// for(Index var = 0; var < variableIndex.size(); ++var) {
// Conditional::shared_ptr conditional(EliminateOneSymbolic(eliminationGraph, variableIndex, var));
// if(conditional) // Will be NULL if the variable did not appear in the factor graph.
// bayesnet->push_back(conditional);
// }
//
// return bayesnet;
//}
/* ************************************************************************* */
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
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");
// Remove the original factors

154
inference/inference.cpp
View File

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

11
inference/inference.h
View File

@ -29,9 +29,6 @@
namespace gtsam {
class Factor;
class Conditional;
class Inference {
private:
/* Static members only, private constructor */
@ -51,8 +48,8 @@ class Conditional;
* variables in order starting from 0. Special fast version for symbolic
* elimination.
*/
template<class FACTOR>
static BayesNet<Conditional>::shared_ptr EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph);
// template<class FACTOR>
// static BayesNet<Conditional>::shared_ptr EliminateSymbolic(const FactorGraph<FACTOR>& factorGraph);
/**
* 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
* symbolic factor graphs.
*/
static boost::shared_ptr<Conditional>
EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var);
// static boost::shared_ptr<Conditional>
// EliminateOneSymbolic(FactorGraph<Factor>& factorGraph, VariableIndex<>& variableIndex, Index var);
/**
* 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/BayesTree-inl.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/inference-inl.h>
using namespace gtsam;
typedef BayesTree<Conditional> SymbolicBayesTree;
typedef BayesTree<IndexConditional> SymbolicBayesTree;
///* ************************************************************************* */
//// SLAM example from RSS sqrtSAM paper
static const Index _x3_=0, _x2_=1, _x1_=2, _l2_=3, _l1_=4;
//Conditional::shared_ptr
// x3(new Conditional(_x3_)),
// x2(new Conditional(_x2_,_x3_)),
// x1(new Conditional(_x1_,_x2_,_x3_)),
// l1(new Conditional(_l1_,_x1_,_x2_)),
// l2(new Conditional(_l2_,_x1_,_x3_));
//IndexConditional::shared_ptr
// x3(new IndexConditional(_x3_)),
// x2(new IndexConditional(_x2_,_x3_)),
// x1(new IndexConditional(_x1_,_x2_,_x3_)),
// l1(new IndexConditional(_l1_,_x1_,_x2_)),
// l2(new IndexConditional(_l2_,_x1_,_x3_));
//
//// Bayes Tree for sqrtSAM example
//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
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
Conditional::shared_ptr
B(new Conditional(_B_)),
L(new Conditional(_L_, _B_)),
E(new Conditional(_E_, _L_, _B_)),
S(new Conditional(_S_, _L_, _B_)),
T(new Conditional(_T_, _E_, _L_)),
X(new Conditional(_X_, _E_));
IndexConditional::shared_ptr
B(new IndexConditional(_B_)),
L(new IndexConditional(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_));
// Bayes Tree for Asia example
SymbolicBayesTree createAsiaSymbolicBayesTree() {
@ -89,15 +89,15 @@ TEST( BayesTree, constructor )
LONGS_EQUAL(4,bayesTree.size());
// Check root
BayesNet<Conditional> expected_root;
BayesNet<IndexConditional> expected_root;
expected_root.push_back(E);
expected_root.push_back(L);
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));
// 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(T);
ASIA.push_back(S);
@ -145,13 +145,13 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental.
TEST( BayesTree, removePath )
{
const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
Conditional::shared_ptr
A(new Conditional(_A_)),
B(new Conditional(_B_, _A_)),
C(new Conditional(_C_, _A_)),
D(new Conditional(_D_, _C_)),
E(new Conditional(_E_, _B_)),
F(new Conditional(_F_, _E_));
IndexConditional::shared_ptr
A(new IndexConditional(_A_)),
B(new IndexConditional(_B_, _A_)),
C(new IndexConditional(_C_, _A_)),
D(new IndexConditional(_D_, _C_)),
E(new IndexConditional(_E_, _B_)),
F(new IndexConditional(_F_, _E_));
SymbolicBayesTree bayesTree;
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
bayesTree.insert(A);
@ -170,7 +170,7 @@ TEST( BayesTree, removePath )
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree[_D_], bayesTree[_E_];
BayesNet<Conditional> bn;
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree[_C_], bn, orphans);
SymbolicFactorGraph factors(bn);
@ -183,7 +183,7 @@ TEST( BayesTree, removePath )
SymbolicBayesTree::Cliques expectedOrphans2;
expectedOrphans2 += bayesTree[_F_];
BayesNet<Conditional> bn2;
BayesNet<IndexConditional> bn2;
SymbolicBayesTree::Cliques orphans2;
bayesTree.removePath(bayesTree[_E_], bn2, orphans2);
SymbolicFactorGraph factors2(bn2);
@ -197,7 +197,7 @@ TEST( BayesTree, removePath2 )
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique B
BayesNet<Conditional> bn;
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree[_B_], bn, orphans);
SymbolicFactorGraph factors(bn);
@ -219,7 +219,7 @@ TEST( BayesTree, removePath3 )
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
// Call remove-path with clique S
BayesNet<Conditional> bn;
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree[_S_], bn, orphans);
SymbolicFactorGraph factors(bn);
@ -242,10 +242,10 @@ TEST( BayesTree, removeTop )
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
// 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
BayesNet<Conditional> bn;
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
@ -263,8 +263,8 @@ TEST( BayesTree, removeTop )
CHECK(assert_equal(expectedOrphans, orphans));
// Try removeTop again with a factor that should not change a thing
boost::shared_ptr<Factor> newFactor2(new Factor(_B_));
BayesNet<Conditional> bn2;
boost::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
BayesNet<IndexConditional> bn2;
SymbolicBayesTree::Cliques orphans2;
keys.clear(); keys += _B_;
bayesTree.removeTop(keys, bn2, orphans2);
@ -286,7 +286,7 @@ TEST( BayesTree, removeTop2 )
newFactors.push_factor(_S_);
// Remove the contaminated part of the Bayes tree
BayesNet<Conditional> bn;
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
list<Index> keys; keys += _B_,_S_;
bayesTree.removeTop(keys, bn, orphans);
@ -309,11 +309,11 @@ TEST( BayesTree, removeTop3 )
{
const Index _x4_=5, _l5_=6;
// simple test case that failed after COLAMD was fixed/activated
Conditional::shared_ptr
X(new Conditional(_l5_)),
A(new Conditional(_x4_, _l5_)),
B(new Conditional(_x2_, _x4_)),
C(new Conditional(_x3_, _x2_));
IndexConditional::shared_ptr
X(new IndexConditional(_l5_)),
A(new IndexConditional(_x4_, _l5_)),
B(new IndexConditional(_x2_, _x4_)),
C(new IndexConditional(_x3_, _x2_));
// Ordering newOrdering;
// newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
@ -326,7 +326,7 @@ TEST( BayesTree, removeTop3 )
// remove all
list<Index> keys;
keys += _l5_, _x2_, _x3_, _x4_;
BayesNet<Conditional> bn;
BayesNet<IndexConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removeTop(keys, bn, orphans);
SymbolicFactorGraph factors(bn);
@ -356,7 +356,7 @@ TEST( BayesTree, insert )
// Ordering ordering2; ordering2 += _x1_, _x2_;
// Ordering ordering3; ordering3 += _x6_, _x5_;
BayesNet<Conditional> bn1, bn2, bn3;
BayesNet<IndexConditional> bn1, bn2, bn3;
bn1 = *Inference::EliminateUntil(fg1, _x4_+1);
bn2 = *Inference::EliminateUntil(fg2, _x2_+1);
bn3 = *Inference::EliminateUntil(fg3, _x5_+1);
@ -383,7 +383,7 @@ TEST( BayesTree, insert )
fg.push_factor(_x6_, _x4_);
// Ordering ordering; ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
BayesNet<Conditional> bn;
BayesNet<IndexConditional> bn;
bn = *Inference::Eliminate(fg);
SymbolicBayesTree expected(bn);
CHECK(assert_equal(expected, actual));

38
inference/tests/testConditional.cpp
View File

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

14
inference/tests/testEliminationTree.cpp
View File

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

2
inference/tests/testFactorGraph.cpp
View File

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

4
inference/tests/testISAM.cpp
View File

@ -24,14 +24,14 @@ using namespace boost::assign;
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/ISAM-inl.h>
using namespace std;
using namespace gtsam;
typedef ISAM<Conditional> SymbolicISAM;
typedef ISAM<IndexConditional> SymbolicISAM;
/* ************************************************************************* */
// 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/ClusterTree-inl.h>
#include <gtsam/inference/JunctionTree-inl.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
using namespace gtsam;
typedef JunctionTree<SymbolicFactorGraph> SymbolicJunctionTree;
typedef BayesTree<Conditional> SymbolicBayesTree;
typedef BayesTree<IndexConditional> SymbolicBayesTree;
/* ************************************************************************* *
* x1 - x2 - x3 - x4
@ -83,7 +83,7 @@ TEST( JunctionTree, eliminate)
SymbolicJunctionTree jt(fg);
SymbolicBayesTree::sharedClique actual = jt.eliminate();
BayesNet<Conditional> bn = *Inference::Eliminate(fg);
BayesNet<IndexConditional> bn = *Inference::Eliminate(fg);
SymbolicBayesTree expected(bn);
// 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 _C_ = 3;
Conditional::shared_ptr
B(new Conditional(_B_)),
L(new Conditional(_L_, _B_));
IndexConditional::shared_ptr
B(new IndexConditional(_B_)),
L(new IndexConditional(_L_, _B_));
/* ************************************************************************* */
TEST( SymbolicBayesNet, equals )
{
BayesNet<Conditional> f1;
BayesNet<IndexConditional> f1;
f1.push_back(B);
f1.push_back(L);
BayesNet<Conditional> f2;
BayesNet<IndexConditional> f2;
f2.push_back(L);
f2.push_back(B);
CHECK(f1.equals(f1));
@ -53,18 +53,18 @@ TEST( SymbolicBayesNet, equals )
/* ************************************************************************* */
TEST( SymbolicBayesNet, pop_front )
{
Conditional::shared_ptr
A(new Conditional(_A_,_B_,_C_)),
B(new Conditional(_B_,_C_)),
C(new Conditional(_C_));
IndexConditional::shared_ptr
A(new IndexConditional(_A_,_B_,_C_)),
B(new IndexConditional(_B_,_C_)),
C(new IndexConditional(_C_));
// Expected after pop_front
BayesNet<Conditional> expected;
BayesNet<IndexConditional> expected;
expected.push_back(B);
expected.push_back(C);
// Actual
BayesNet<Conditional> actual;
BayesNet<IndexConditional> actual;
actual.push_back(A);
actual.push_back(B);
actual.push_back(C);
@ -76,24 +76,24 @@ TEST( SymbolicBayesNet, pop_front )
/* ************************************************************************* */
TEST( SymbolicBayesNet, combine )
{
Conditional::shared_ptr
A(new Conditional(_A_,_B_,_C_)),
B(new Conditional(_B_,_C_)),
C(new Conditional(_C_));
IndexConditional::shared_ptr
A(new IndexConditional(_A_,_B_,_C_)),
B(new IndexConditional(_B_,_C_)),
C(new IndexConditional(_C_));
// p(A|BC)
BayesNet<Conditional> p_ABC;
BayesNet<IndexConditional> p_ABC;
p_ABC.push_back(A);
// P(BC)=P(B|C)P(C)
BayesNet<Conditional> p_BC;
BayesNet<IndexConditional> p_BC;
p_BC.push_back(B);
p_BC.push_back(C);
// P(ABC) = P(A|BC) P(BC)
p_ABC.push_back(p_BC);
BayesNet<Conditional> expected;
BayesNet<IndexConditional> expected;
expected.push_back(A);
expected.push_back(B);
expected.push_back(C);

20
inference/tests/testSymbolicFactor.cpp
View File

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

32
inference/tests/testSymbolicFactorGraph.cpp
View File

@ -11,7 +11,7 @@
/**
* @file testSymbolicFactorGraph.cpp
* @brief Unit tests for a symbolic Factor Graph
* @brief Unit tests for a symbolic IndexFactor Graph
* @author Frank Dellaert
*/
@ -22,7 +22,7 @@ using namespace boost::assign;
#include <gtsam/inference/SymbolicFactorGraph.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/inference-inl.h>
@ -43,7 +43,7 @@ TEST( SymbolicFactorGraph, EliminateOne )
VariableIndex<> variableIndex(fg);
Inference::EliminateOne(fg, variableIndex, vx2);
SymbolicFactorGraph expected;
expected.push_back(boost::shared_ptr<Factor>());
expected.push_back(boost::shared_ptr<IndexFactor>());
expected.push_factor(vx1);
CHECK(assert_equal(expected, fg));
@ -59,11 +59,11 @@ TEST( SymbolicFactorGraph, constructFromBayesNet )
expected.push_factor(vx1);
// create Bayes Net
Conditional::shared_ptr x2(new Conditional(vx2, vx1, vl1));
Conditional::shared_ptr l1(new Conditional(vx1, vl1));
Conditional::shared_ptr x1(new Conditional(vx1));
IndexConditional::shared_ptr x2(new IndexConditional(vx2, vx1, vl1));
IndexConditional::shared_ptr l1(new IndexConditional(vx1, vl1));
IndexConditional::shared_ptr x1(new IndexConditional(vx1));
BayesNet<Conditional> bayesNet;
BayesNet<IndexConditional> bayesNet;
bayesNet.push_back(x2);
bayesNet.push_back(l1);
bayesNet.push_back(x1);
@ -109,7 +109,7 @@ TEST( SymbolicFactorGraph, push_back )
//
//public:
//
// typedef boost::shared_ptr<Factor> sharedFactor;
// typedef boost::shared_ptr<IndexFactor> sharedFactor;
// typedef boost::shared_ptr<ETree> shared_ptr;
//
//private:
@ -118,7 +118,7 @@ TEST( SymbolicFactorGraph, push_back )
// list<sharedFactor> factors_; /** factors associated with root */
// 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
@ -146,11 +146,11 @@ TEST( SymbolicFactorGraph, push_back )
// // Make the conditional from the key and separator, and insert it in Bayes net
// vector<Index> 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);
//
// // 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);
// }
//
@ -366,11 +366,11 @@ TEST( SymbolicFactorGraph, push_back )
//TEST( SymbolicFactorGraph, eliminate )
//{
// // create expected Chordal bayes Net
// Conditional::shared_ptr c0(new Conditional(0, 1, 2));
// Conditional::shared_ptr c1(new Conditional(1, 2, 4));
// Conditional::shared_ptr c2(new Conditional(2, 4));
// Conditional::shared_ptr c3(new Conditional(3, 4));
// Conditional::shared_ptr c4(new Conditional(4));
// IndexConditional::shared_ptr c0(new IndexConditional(0, 1, 2));
// IndexConditional::shared_ptr c1(new IndexConditional(1, 2, 4));
// IndexConditional::shared_ptr c2(new IndexConditional(2, 4));
// IndexConditional::shared_ptr c3(new IndexConditional(3, 4));
// IndexConditional::shared_ptr c4(new IndexConditional(4));
//
// SymbolicBayesNet expected;
// expected.push_back(c3);

2
inference/tests/timeSymbolMaps.cpp
View File

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

10
linear/GaussianConditional.cpp
View File

@ -33,11 +33,11 @@ namespace gtsam {
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) :
Conditional(key), rsd_(matrix_), sigmas_(sigmas) {
IndexConditional(key), rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
size_t dims[] = { R.size2(), 1 };
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,
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());
size_t dims[] = { R.size2(), S.size2(), 1 };
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,
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());
size_t dims[] = { R.size2(), S.size2(), T.size2(), 1 };
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) :
rsd_(matrix_), sigmas_(sigmas) {
assert(R.size1() <= R.size2());
Conditional::nrFrontals_ = 1;
IndexConditional::nrFrontals_ = 1;
keys_.resize(1+parents.size());
size_t dims[1+parents.size()+1];
dims[0] = R.size2();

6
linear/GaussianConditional.h
View File

@ -25,7 +25,7 @@
#include <boost/numeric/ublas/triangular.hpp>
#include <gtsam/base/types.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/Matrix.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.
* The negative log-probability is given by || Rx - (d - Sy - Tz - ...)||^2
*/
class GaussianConditional : public Conditional {
class GaussianConditional : public IndexConditional {
public:
typedef GaussianFactor FactorType;
typedef GaussianFactor Factor;
typedef boost::shared_ptr<GaussianConditional> shared_ptr;
/** 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 {
#ifndef NDEBUG
Factor::assertInvariants();
IndexFactor::assertInvariants();
assert((keys_.size() == 0 && Ab_.size1() == 0 && Ab_.nBlocks() == 0) || keys_.size()+1 == Ab_.nBlocks());
#endif
}
/* ************************************************************************* */
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_);
assertInvariants();
}
@ -71,7 +71,7 @@ GaussianFactor::GaussianFactor(const Vector& b_in) : firstNonzeroBlocks_(b_in.si
/* ************************************************************************* */
GaussianFactor::GaussianFactor(Index i1, const Matrix& A1,
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};
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+2, b.size()));
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,
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};
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+3, b.size()));
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,
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};
Ab_.copyStructureFrom(ab_type(matrix_, dims, dims+4, b.size()));
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_);
// todo SL: make firstNonzeroCols triangular?
firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions
@ -590,11 +590,11 @@ struct _RowSource {
};
/* 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 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_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_deque>&, const vector<size_t>&, const vector<Index>&, const std::vector<std::vector<size_t> >&);
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 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);
}
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 debug = false;

9
linear/GaussianFactor.h
View File

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

2
linear/VectorBTree.h
View File

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

8
nonlinear/Makefile.am
View File

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

30
nonlinear/NonlinearFactor.h
View File

@ -26,7 +26,7 @@
#include <boost/shared_ptr.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/Matrix.h>
#include <gtsam/linear/SharedGaussian.h>
@ -138,7 +138,7 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const = 0;
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0;
private:
@ -204,7 +204,7 @@ namespace gtsam {
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 {
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
* variable indices.
*/
virtual Factor::shared_ptr symbolic(const Ordering& ordering) const {
return Factor::shared_ptr(new Factor(ordering[key_]));
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
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
* 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_];
if(var1 < var2)
return Factor::shared_ptr(new Factor(var1, var2));
return IndexFactor::shared_ptr(new IndexFactor(var1, var2));
else
return Factor::shared_ptr(new Factor(var2, var1));
return IndexFactor::shared_ptr(new IndexFactor(var2, var1));
}
/** methods to retrieve both keys */
@ -519,20 +519,20 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the
* 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_];
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)
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)
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)
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)
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)
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
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 {
// Create symbolic graph and initial (iterator) ordering
FactorGraph<Factor>::shared_ptr symbolic;
SymbolicFactorGraph::shared_ptr symbolic;
Ordering::shared_ptr ordering;
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(
const Values& config, const Ordering& ordering) const {
// Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new FactorGraph<Factor>);
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
symbolicfg->push_back(factor->symbolic(ordering));

2
slam/saveGraph.h
View File

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

8
tests/testGaussianFactorGraph.cpp
View File

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

18
tests/testGaussianJunctionTree.cpp
View File

@ -69,15 +69,15 @@ TEST( GaussianJunctionTree, constructor2 )
list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
GaussianJunctionTree::sharedClique child0 = *child0it;
GaussianJunctionTree::sharedClique child1 = *child1it;
CHECK(assert_equal(frontal2, child0->frontal));
CHECK(assert_equal(sep2, child0->separator));
LONGS_EQUAL(4, child0->size());
CHECK(assert_equal(frontal3, child0->children().front()->frontal));
CHECK(assert_equal(sep3, child0->children().front()->separator));
LONGS_EQUAL(2, child0->children().front()->size());
CHECK(assert_equal(frontal4, child1->frontal));
CHECK(assert_equal(sep4, child1->separator));
LONGS_EQUAL(2, child1->size());
CHECK(assert_equal(frontal2, child1->frontal));
CHECK(assert_equal(sep2, child1->separator));
LONGS_EQUAL(4, child1->size());
CHECK(assert_equal(frontal3, child1->children().front()->frontal));
CHECK(assert_equal(sep3, child1->children().front()->separator));
LONGS_EQUAL(2, child1->children().front()->size());
CHECK(assert_equal(frontal4, child0->frontal));
CHECK(assert_equal(sep4, child0->separator));
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/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/nonlinear/Ordering.h>
using namespace std;
@ -34,20 +33,20 @@ using namespace gtsam;
using namespace example;
//Symbol _B_('B', 0), _L_('L', 0);
//Conditional::shared_ptr
// B(new Conditional(_B_)),
// L(new Conditional(_L_, _B_));
//IndexConditional::shared_ptr
// B(new IndexConditional(_B_)),
// L(new IndexConditional(_L_, _B_));
/* ************************************************************************* */
TEST( SymbolicBayesNet, constructor )
{
Ordering o; o += "x2","l1","x1";
// Create manually
Conditional::shared_ptr
x2(new Conditional(o["x2"],o["l1"], o["x1"])),
l1(new Conditional(o["l1"],o["x1"])),
x1(new Conditional(o["x1"]));
BayesNet<Conditional> expected;
IndexConditional::shared_ptr
x2(new IndexConditional(o["x2"],o["l1"], o["x1"])),
l1(new IndexConditional(o["l1"],o["x1"])),
x1(new IndexConditional(o["x1"]));
BayesNet<IndexConditional> expected;
expected.push_back(x2);
expected.push_back(l1);
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/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/inference/inference-inl.h>
@ -117,10 +116,10 @@ TEST( SymbolicFactorGraph, eliminateOne )
// eliminate
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
Conditional expected(o["x1"],o["l1"],o["x2"]);
// create expected symbolic IndexConditional
IndexConditional expected(o["x1"],o["l1"],o["x2"]);
CHECK(assert_equal(expected,*actual));
}
@ -131,9 +130,9 @@ TEST( SymbolicFactorGraph, eliminate )
Ordering o; o += "x2","l1","x1";
// create expected Chordal bayes Net
Conditional::shared_ptr x2(new Conditional(o["x2"], o["l1"], o["x1"]));
Conditional::shared_ptr l1(new Conditional(o["l1"], o["x1"]));
Conditional::shared_ptr x1(new Conditional(o["x1"]));
IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"]));
IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"]));
IndexConditional::shared_ptr x1(new IndexConditional(o["x1"]));
SymbolicBayesNet expected;
expected.push_back(x2);