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

Template arguements capitilized

release/4.3a0
Rahul Sawhney 2010-10-20 01:10:27 +00:00
parent 240889084b
commit 7d4f1ad268
18 changed files with 258 additions and 261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
inference/BayesNet.h
View File

@ -135,8 +135,8 @@ namespace gtsam {
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_);
}
}; // BayesNet

51
inference/BayesTree-inl.h
View File

@ -266,7 +266,7 @@ namespace gtsam {
// TODO, why do we actually return a shared pointer, why does eliminate?
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FactorGraph>
template<class FACTORGRAPH>
BayesNet<CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R) {
// A first base case is when this clique or its parent is the root,
@ -281,13 +281,13 @@ namespace gtsam {
// The parent clique has a CONDITIONAL for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form
FactorGraph p_Fp_Sp(*parent);
FACTORGRAPH p_Fp_Sp(*parent);
// If not the base case, obtain the parent shortcut P(Sp|R) as factors
FactorGraph p_Sp_R(parent->shortcut<FactorGraph>(R));
FACTORGRAPH p_Sp_R(parent->shortcut<FACTORGRAPH>(R));
// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
FactorGraph p_Cp_R = combine(p_Fp_Sp, p_Sp_R);
FACTORGRAPH p_Cp_R = combine(p_Fp_Sp, p_Sp_R);
// Eliminate into a Bayes net with ordering designed to integrate out
// any variables not in *our* separator. Variables to integrate out must be
@ -319,10 +319,10 @@ namespace gtsam {
BOOST_FOREACH(Index key, separator) ordering.push_back(key);
// eliminate to get marginal
typename FactorGraph::variableindex_type varIndex(p_Cp_R);
typename FACTORGRAPH::variableindex_type varIndex(p_Cp_R);
Permutation toFront = Permutation::PullToFront(ordering, varIndex.size());
Permutation::shared_ptr toFrontInverse(toFront.inverse());
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, p_Cp_R) {
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, p_Cp_R) {
factor->permuteWithInverse(*toFrontInverse);
}
varIndex.permute(toFront);
@ -346,19 +346,18 @@ namespace gtsam {
// Because the root clique could be very big.
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FactorGraph>
FactorGraph
BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
template<class FACTORGRAPH>
FACTORGRAPH BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
// If we are the root, just return this root
if (R.get()==this) return *R;
// Combine P(F|S), P(S|R), and P(R)
BayesNet<CONDITIONAL> p_FSR = this->shortcut<FactorGraph>(R);
BayesNet<CONDITIONAL> p_FSR = this->shortcut<FACTORGRAPH>(R);
p_FSR.push_front(*this);
p_FSR.push_back(*R);
// Find marginal on the keys we are interested in
return Inference::Marginal(FactorGraph(p_FSR), keys());
return Inference::Marginal(FACTORGRAPH(p_FSR), keys());
}
// /* ************************************************************************* */
@ -556,9 +555,9 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
template<class Container>
inline Index BayesTree<CONDITIONAL>::findParentClique(const Container& parents) const {
typename Container::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
template<class CONTAINER>
inline Index BayesTree<CONDITIONAL>::findParentClique(const CONTAINER& parents) const {
typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent;
@ -690,29 +689,28 @@ namespace gtsam {
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FactorGraph>
FactorGraph
BayesTree<CONDITIONAL>::marginal(Index key) const {
template<class FACTORGRAPH>
FACTORGRAPH BayesTree<CONDITIONAL>::marginal(Index key) const {
// get clique containing key
sharedClique clique = (*this)[key];
// calculate or retrieve its marginal
FactorGraph cliqueMarginal = clique->marginal<FactorGraph>(root_);
FACTORGRAPH cliqueMarginal = clique->marginal<FACTORGRAPH>(root_);
// Reorder so that only the requested key is not eliminated
typename FactorGraph::variableindex_type varIndex(cliqueMarginal);
typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal);
vector<Index> keyAsVector(1); keyAsVector[0] = key;
Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size()));
Permutation::shared_ptr toBackInverse(toBack.inverse());
varIndex.permute(toBack);
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, cliqueMarginal) {
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
factor->permuteWithInverse(*toBackInverse);
}
// partially eliminate, remaining factor graph is requested marginal
Inference::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex);
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, cliqueMarginal) {
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
if(factor)
factor->permuteWithInverse(toBack);
}
@ -721,12 +719,11 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FactorGraph>
BayesNet<CONDITIONAL>
BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
template<class FACTORGRAPH>
BayesNet<CONDITIONAL> BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
// calculate marginal as a factor graph
FactorGraph fg = this->marginal<FactorGraph>(key);
FACTORGRAPH fg = this->marginal<FACTORGRAPH>(key);
// eliminate further to Bayes net
return *Inference::Eliminate(fg);
@ -808,8 +805,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class CONDITIONAL>
template<class Container>
void BayesTree<CONDITIONAL>::removeTop(const Container& keys,
template<class CONTAINER>
void BayesTree<CONDITIONAL>::removeTop(const CONTAINER& keys,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) {
// process each key of the new factor

20
inference/BayesTree.h
View File

@ -107,12 +107,12 @@ namespace gtsam {
/** return the conditional P(S|Root) on the separator given the root */
// TODO: create a cached version
template<class FactorGraph>
template<class FACTORGRAPH>
BayesNet<CONDITIONAL> shortcut(shared_ptr root);
/** return the marginal P(C) of the clique */
template<class FactorGraph>
FactorGraph marginal(shared_ptr root);
template<class FACTORGRAPH>
FACTORGRAPH marginal(shared_ptr root);
// /** return the joint P(C1,C2), where C1==this. TODO: not a method? */
// template<class Factor>
@ -233,8 +233,8 @@ namespace gtsam {
* Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering.
*/
template<class Container>
Index findParentClique(const Container& parents) const;
template<class CONTAINER>
Index findParentClique(const CONTAINER& parents) const;
/** number of cliques */
inline size_t size() const {
@ -257,11 +257,11 @@ namespace gtsam {
CliqueData getCliqueData() const;
/** return marginal on any variable */
template<class FactorGraph>
FactorGraph marginal(Index key) const;
template<class FACTORGRAPH>
FACTORGRAPH marginal(Index key) const;
/** return marginal on any variable, as a Bayes Net */
template<class FactorGraph>
template<class FACTORGRAPH>
BayesNet<CONDITIONAL> marginalBayesNet(Index key) const;
// /** return joint on two variables */
@ -299,8 +299,8 @@ namespace gtsam {
* Given a list of keys, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
template<class Container>
void removeTop(const Container& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
template<class CONTAINER>
void removeTop(const CONTAINER& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
}; // BayesTree

8
inference/ClusterTree-inl.h
View File

@ -37,16 +37,16 @@ namespace gtsam {
/* ************************************************************************* */
template<class FG>
template<typename FrontalIt, typename SeparatorIt>
template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster(
const FG& fg, FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator) :
const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */
template<class FG>
template<typename FrontalIt, typename SeparatorIt>
template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster(
FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator) :
FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator) :
frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */

8
inference/ClusterTree.h
View File

@ -66,12 +66,12 @@ namespace gtsam {
Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator);
/* Create a node with several frontal variables */
template<typename FrontalIt, typename SeparatorIt>
Cluster(const FG& fg, FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator);
template<typename FRONTALIT, typename SEPARATORIT>
Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
/* Create a node with several frontal variables */
template<typename FrontalIt, typename SeparatorIt>
Cluster(FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator);
template<typename FRONTALIT, typename SEPARATORIT>
Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
// print the object
void print(const std::string& indent) const;

4
inference/Conditional.h
View File

@ -187,8 +187,8 @@ protected:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
}
};

8
inference/Factor.h
View File

@ -77,7 +77,7 @@ public:
FactorBase(const Conditional& c);
/** Constructor from a collection of keys */
template<class KeyIterator> FactorBase(KeyIterator beginKey, KeyIterator endKey) :
template<class KEYITERATOR> FactorBase(KEYITERATOR beginKey, KEYITERATOR endKey) :
keys_(beginKey, endKey) { assertInvariants(); }
/** Default constructor for I/O */
@ -102,9 +102,9 @@ public:
/** 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 DERIVED, class FactorGraphType, class VariableIndexStorage>
static typename DERIVED::shared_ptr Combine(const FactorGraphType& factorGraph,
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors,
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<KEY>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
return typename DERIVED::shared_ptr(new DERIVED(variables.begin(), variables.end())); }

6
inference/FactorGraph-inl.h
View File

@ -103,10 +103,10 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class FactorGraph>
FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) {
template<class FACTORGRAPH>
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2) {
// create new linear factor graph equal to the first one
FactorGraph fg = fg1;
FACTORGRAPH fg = fg1;
// add the second factors_ in the graph
fg.push_back(fg2);

46
inference/FactorGraph.h
View File

@ -61,23 +61,23 @@ namespace gtsam {
FactorGraph() {}
/** convert from Bayes net */
template<class Conditional>
FactorGraph(const BayesNet<Conditional>& bayesNet);
template<class CONDITIONAL>
FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
/** convert from a derived type */
template<class DerivedFactor>
FactorGraph(const FactorGraph<DerivedFactor>& factorGraph);
template<class DERIVEDFACTOR>
FactorGraph(const FactorGraph<DERIVEDFACTOR>& factorGraph);
/** Add a factor */
template<class DerivedFactor>
void push_back(const boost::shared_ptr<DerivedFactor>& factor);
template<class DERIVEDFACTOR>
void push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor);
/** push back many factors */
void push_back(const FactorGraph<FACTOR>& factors);
/** push back many factors with an iterator */
template<typename Iterator>
void push_back(Iterator firstFactor, Iterator lastFactor);
template<typename ITERATOR>
void push_back(ITERATOR firstFactor, ITERATOR lastFactor);
/** ------------------ Querying Factor Graphs ---------------------------- */
@ -131,8 +131,8 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(factors_);
}
}; // FactorGraph
@ -143,8 +143,8 @@ namespace gtsam {
* @param const &fg2 Linear factor graph
* @return a new combined factor graph
*/
template<class FactorGraph>
FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2);
template<class FACTORGRAPH>
FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2);
/**
* These functions are defined here because they are templated on an
@ -155,10 +155,10 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
template<class DerivedFactor>
FactorGraph<FACTOR>::FactorGraph(const FactorGraph<DerivedFactor>& factorGraph) {
template<class DERIVEDFACTOR>
FactorGraph<FACTOR>::FactorGraph(const FactorGraph<DERIVEDFACTOR>& factorGraph) {
factors_.reserve(factorGraph.size());
BOOST_FOREACH(const typename DerivedFactor::shared_ptr& factor, factorGraph) {
BOOST_FOREACH(const typename DERIVEDFACTOR::shared_ptr& factor, factorGraph) {
if(factor)
this->push_back(sharedFactor(new FACTOR(*factor)));
else
@ -168,18 +168,18 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
template<class Conditional>
FactorGraph<FACTOR>::FactorGraph(const BayesNet<Conditional>& bayesNet) {
template<class CONDITIONAL>
FactorGraph<FACTOR>::FactorGraph(const BayesNet<CONDITIONAL>& bayesNet) {
factors_.reserve(bayesNet.size());
BOOST_FOREACH(const typename Conditional::shared_ptr& cond, bayesNet) {
BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, bayesNet) {
this->push_back(sharedFactor(new FACTOR(*cond)));
}
}
/* ************************************************************************* */
template<class FACTOR>
template<class DerivedFactor>
inline void FactorGraph<FACTOR>::push_back(const boost::shared_ptr<DerivedFactor>& factor) {
template<class DERIVEDFACTOR>
inline void FactorGraph<FACTOR>::push_back(const boost::shared_ptr<DERIVEDFACTOR>& factor) {
#ifndef NDEBUG
factors_.push_back(boost::dynamic_pointer_cast<FACTOR>(factor)); // add the actual factor
#else
@ -189,9 +189,9 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
template<typename Iterator>
void FactorGraph<FACTOR>::push_back(Iterator firstFactor, Iterator lastFactor) {
Iterator factor = firstFactor;
template<typename ITERATOR>
void FactorGraph<FACTOR>::push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
ITERATOR factor = firstFactor;
while(factor != lastFactor)
this->push_back(*(factor++));
}

30
inference/ISAM-inl.h
View File

@ -28,32 +28,32 @@ namespace gtsam {
using namespace std;
/** Create an empty Bayes Tree */
template<class Conditional>
ISAM<Conditional>::ISAM() : BayesTree<Conditional>() {}
template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {}
/** Create a Bayes Tree from a Bayes Net */
template<class Conditional>
ISAM<Conditional>::ISAM(const BayesNet<Conditional>& bayesNet) :
BayesTree<Conditional>(bayesNet) {}
template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM(const BayesNet<CONDITIONAL>& bayesNet) :
BayesTree<CONDITIONAL>(bayesNet) {}
/* ************************************************************************* */
template<class Conditional>
template<class FactorGraph>
void ISAM<Conditional>::update_internal(const FactorGraph& newFactors, Cliques& orphans) {
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
// Remove the contaminated part of the Bayes tree
BayesNet<Conditional> bn;
BayesNet<CONDITIONAL> bn;
removeTop(newFactors.keys(), bn, orphans);
FactorGraph factors(bn);
FACTORGRAPH factors(bn);
// add the factors themselves
factors.push_back(newFactors);
// eliminate into a Bayes net
typename BayesNet<Conditional>::shared_ptr bayesNet = Inference::Eliminate(factors);
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = Inference::Eliminate(factors);
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<Conditional>::const_reverse_iterator rit;
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
this->insert(*rit);
@ -64,9 +64,9 @@ namespace gtsam {
}
template<class Conditional>
template<class FactorGraph>
void ISAM<Conditional>::update(const FactorGraph& newFactors) {
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update(const FACTORGRAPH& newFactors) {
Cliques orphans;
this->update_internal(newFactors, orphans);
}

18
inference/ISAM.h
View File

@ -35,8 +35,8 @@
namespace gtsam {
template<class Conditional>
class ISAM: public BayesTree<Conditional> {
template<class CONDITIONAL>
class ISAM: public BayesTree<CONDITIONAL> {
public:
@ -44,19 +44,19 @@ namespace gtsam {
ISAM();
/** Create a Bayes Tree from a Bayes Net */
ISAM(const BayesNet<Conditional>& bayesNet);
ISAM(const BayesNet<CONDITIONAL>& bayesNet);
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique;
typedef typename BayesTree<Conditional>::Cliques Cliques;
typedef typename BayesTree<CONDITIONAL>::Cliques Cliques;
/**
* iSAM. (update_internal provides access to list of orphans for drawing purposes)
*/
template<class FactorGraph>
void update_internal(const FactorGraph& newFactors, Cliques& orphans);
template<class FactorGraph>
void update(const FactorGraph& newFactors);
template<class FACTORGRAPH>
void update_internal(const FACTORGRAPH& newFactors, Cliques& orphans);
template<class FACTORGRAPH>
void update(const FACTORGRAPH& newFactors);
}; // ISAM

6
inference/IndexFactor.h
View File

@ -63,9 +63,9 @@ public:
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,
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); }

18
inference/Permutation.h
View File

@ -156,22 +156,22 @@ protected:
/**
* Definition of Permuted class, see above comment for details.
*/
template<typename Container, typename ValueType = typename Container::value_reference_type>
template<typename CONTAINER, typename VALUETYPE = typename CONTAINER::value_reference_type>
class Permuted {
Permutation permutation_;
Container& container_;
CONTAINER& container_;
public:
typedef ValueType value_type;
typedef VALUETYPE value_type;
/** Construct as a permuted view on the Container. The permutation is copied
* but only a reference to the container is stored.
*/
Permuted(const Permutation& permutation, Container& container) : permutation_(permutation), container_(container) {}
Permuted(const Permutation& permutation, CONTAINER& container) : permutation_(permutation), container_(container) {}
/** Construct as a view on the Container with an identity permutation. Only
* a reference to the container is stored.
*/
Permuted(Container& container) : permutation_(Permutation::Identity(container.size())), container_(container) {}
Permuted(CONTAINER& container) : permutation_(Permutation::Identity(container.size())), container_(container) {}
/** Access the container through the permutation */
value_type operator[](size_t index) const { return container_[permutation_[index]]; }
@ -183,19 +183,19 @@ public:
void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }
/** Access the underlying container */
Container* operator->() { return &container_; }
CONTAINER* operator->() { return &container_; }
/** Access the underlying container (const version) */
const Container* operator->() const { return &container_; }
const CONTAINER* operator->() const { return &container_; }
/** Size of the underlying container */
size_t size() const { return container_.size(); }
/** Access to the underlying container */
Container& container() { return container_; }
CONTAINER& container() { return container_; }
/** Access to the underlying container (const version) */
const Container& container() const { return container_; }
const CONTAINER& container() const { return container_; }
/** Access the underlying permutation */
Permutation& permutation() { return permutation_; }

4
inference/VariableIndex.h
View File

@ -53,7 +53,7 @@ struct _mapped_factor_type {
_mapped_factor_type(size_t _factorIndex, size_t _variablePosition) : factorIndex(_factorIndex), variablePosition(_variablePosition) {}
bool operator==(const _mapped_factor_type& o) const { return factorIndex == o.factorIndex && variablePosition == o.variablePosition; }
};
template<class VariableIndexStorage=VariableIndexStorage_vector>
template<class VARIABLEINDEXSTORAGE=VariableIndexStorage_vector>
class VariableIndex {
public:
@ -64,7 +64,7 @@ public:
typedef typename mapped_type::const_iterator const_factor_iterator;
protected:
typedef typename VariableIndexStorage::template type_factory<mapped_type>::type storage_type;
typedef typename VARIABLEINDEXSTORAGE::template type_factory<mapped_type>::type storage_type;
storage_type indexUnpermuted_;
Permuted<storage_type, typename storage_type::value_type&> index_;
size_t nFactors_;

118
inference/graph-inl.h
View File

@ -30,42 +30,42 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template <class Key>
template <class KEY>
class ordering_key_visitor : public boost::default_bfs_visitor {
public:
ordering_key_visitor(std::list<Key>& ordering_in) : ordering_(ordering_in) {}
ordering_key_visitor(std::list<KEY>& ordering_in) : ordering_(ordering_in) {}
template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
Key key = boost::get(boost::vertex_name, g, v);
KEY key = boost::get(boost::vertex_name, g, v);
ordering_.push_front(key);
}
std::list<Key>& ordering_;
std::list<KEY>& ordering_;
};
/* ************************************************************************* */
template<class Key>
list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map) {
template<class KEY>
list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map) {
typedef typename SGraph<Key>::Vertex SVertex;
typedef typename SGraph<KEY>::Vertex SVertex;
SGraph<Key> g;
SGraph<KEY> g;
SVertex root;
std::map<Key, SVertex> key2vertex;
boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
std::map<KEY, SVertex> key2vertex;
boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
// breadth first visit on the graph
std::list<Key> keys;
ordering_key_visitor<Key> vis(keys);
std::list<KEY> keys;
ordering_key_visitor<KEY> vis(keys);
boost::breadth_first_search(g, root, boost::visitor(vis));
return keys;
}
/* ************************************************************************* */
template<class G, class F, class Key>
SDGraph<Key> toBoostGraph(const G& graph) {
template<class G, class F, class KEY>
SDGraph<KEY> toBoostGraph(const G& graph) {
// convert the factor graph to boost graph
SDGraph<Key> g;
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
map<Key, BoostVertex> key2vertex;
SDGraph<KEY> g;
typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
map<KEY, BoostVertex> key2vertex;
BoostVertex v1, v2;
typename G::const_iterator itFactor;
@ -79,8 +79,8 @@ SDGraph<Key> toBoostGraph(const G& graph) {
boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
if (!factor) continue;
Key key1 = factor->key1();
Key key2 = factor->key2();
KEY key1 = factor->key1();
KEY key2 = factor->key2();
if (key2vertex.find(key1) == key2vertex.end()) {
v1 = add_vertex(key1, g);
@ -102,14 +102,14 @@ SDGraph<Key> toBoostGraph(const G& graph) {
}
/* ************************************************************************* */
template<class G, class V, class Key>
boost::tuple<G, V, map<Key,V> >
predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
template<class G, class V, class KEY>
boost::tuple<G, V, map<KEY,V> >
predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
G g;
map<Key, V> key2vertex;
map<KEY, V> key2vertex;
V v1, v2, root;
Key child, parent;
KEY child, parent;
bool foundRoot = false;
FOREACH_PAIR(child, parent, p_map) {
if (key2vertex.find(child) == key2vertex.end()) {
@ -134,47 +134,47 @@ predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
if (!foundRoot)
throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
else
return boost::tuple<G, V, std::map<Key, V> >(g, root, key2vertex);
return boost::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex);
}
/* ************************************************************************* */
template <class V,class Pose, class Values>
template <class V,class POSE, class VALUES>
class compose_key_visitor : public boost::default_bfs_visitor {
private:
boost::shared_ptr<Values> config_;
boost::shared_ptr<VALUES> config_;
public:
compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
compose_key_visitor(boost::shared_ptr<VALUES> config_in) {config_ = config_in;}
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
typename Values::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename Values::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge);
typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
POSE relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, (*config_)[key_from].compose(relativePose));
}
};
/* ************************************************************************* */
template<class G, class Factor, class Pose, class Values>
boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree,
const Pose& rootPose) {
template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
const POSE& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, typename Values::Key>,
boost::property<boost::edge_weight_t, Pose> > PoseGraph;
boost::property<boost::vertex_name_t, typename VALUES::Key>,
boost::property<boost::edge_weight_t, POSE> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g;
PoseVertex root;
map<typename Values::Key, PoseVertex> key2vertex;
map<typename VALUES::Key, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) =
predecessorMap2Graph<PoseGraph, PoseVertex, typename Values::Key>(tree);
predecessorMap2Graph<PoseGraph, PoseVertex, typename VALUES::Key>(tree);
// attach the relative poses to the edges
PoseEdge edge12, edge21;
@ -188,13 +188,13 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<type
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue;
typename Values::Key key1 = factor->key1();
typename Values::Key key2 = factor->key2();
typename VALUES::Key key1 = factor->key1();
typename VALUES::Key key2 = factor->key2();
PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second;
Pose l1Xl2 = factor->measured();
POSE l1Xl2 = factor->measured();
tie(edge12, found1) = boost::edge(v1, v2, g);
tie(edge21, found2) = boost::edge(v2, v1, g);
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
@ -206,10 +206,10 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<type
}
// compose poses
boost::shared_ptr<Values> config(new Values);
typename Values::Key rootKey = boost::get(boost::vertex_name, g, root);
boost::shared_ptr<VALUES> config(new VALUES);
typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose);
compose_key_visitor<PoseVertex, Pose, Values> vis(config);
compose_key_visitor<PoseVertex, POSE, VALUES> vis(config);
boost::breadth_first_search(g, root, boost::visitor(vis));
return config;
@ -218,22 +218,22 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<type
/* ************************************************************************* */
/* ************************************************************************* */
template<class G, class Key, class Factor2>
PredecessorMap<Key> findMinimumSpanningTree(const G& fg) {
template<class G, class KEY, class FACTOR2>
PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
SDGraph<Key> g = gtsam::toBoostGraph<G, Factor2, Key>(fg);
SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
// find minimum spanning tree
vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g));
vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g));
prim_minimum_spanning_tree(g, &p_map[0]);
// convert edge to string pairs
PredecessorMap<Key> tree;
typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first;
typename vector<typename SDGraph<Key>::Vertex>::iterator vi;
PredecessorMap<KEY> tree;
typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
typename vector<typename SDGraph<KEY>::Vertex>::iterator vi;
for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) {
Key key = boost::get(boost::vertex_name, g, *itVertex);
Key parent = boost::get(boost::vertex_name, g, *vi);
KEY key = boost::get(boost::vertex_name, g, *itVertex);
KEY parent = boost::get(boost::vertex_name, g, *vi);
tree.insert(key, parent);
}
@ -241,8 +241,8 @@ PredecessorMap<Key> findMinimumSpanningTree(const G& fg) {
}
/* ************************************************************************* */
template<class G, class Key, class Factor2>
void split(const G& g, const PredecessorMap<Key>& tree, G& Ab1, G& Ab2) {
template<class G, class KEY, class FACTOR2>
void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
typedef typename G::sharedFactor F ;
@ -256,12 +256,12 @@ void split(const G& g, const PredecessorMap<Key>& tree, G& Ab1, G& Ab2) {
continue;
}
boost::shared_ptr<Factor2> factor2 = boost::dynamic_pointer_cast<
Factor2>(factor);
boost::shared_ptr<FACTOR2> factor2 = boost::dynamic_pointer_cast<
FACTOR2>(factor);
if (!factor2) continue;
Key key1 = factor2->key1();
Key key2 = factor2->key2();
KEY key1 = factor2->key1();
KEY key2 = factor2->key2();
// if the tree contains the key
if ((tree.find(key1) != tree.end() &&
tree.find(key1)->second.compare(key2) == 0) ||

44
inference/graph.h
View File

@ -33,19 +33,19 @@ namespace gtsam {
/**
* SDGraph is undirected graph with variable keys and double edge weights
*/
template<class Key>
template<class KEY>
class SDGraph: public boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, Key>, boost::property<
boost::property<boost::vertex_name_t, KEY>, boost::property<
boost::edge_weight_t, double> > {
public:
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor Vertex;
typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor Vertex;
};
template<class Key>
template<class KEY>
class SGraph : public boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, Key> > {
boost::property<boost::vertex_name_t, KEY> > {
public:
typedef typename boost::graph_traits<SGraph<Key> >::vertex_descriptor Vertex;
typedef typename boost::graph_traits<SGraph<KEY> >::vertex_descriptor Vertex;
};
//typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
@ -53,20 +53,20 @@ namespace gtsam {
/**
* Map from variable key to parent key
*/
template<class Key>
class PredecessorMap: public std::map<Key, Key> {
template<class KEY>
class PredecessorMap: public std::map<KEY, KEY> {
public:
/** convenience insert so we can pass ints for TypedSymbol keys */
inline void insert(const Key& key, const Key& parent) {
std::map<Key, Key>::insert(std::make_pair(key, parent));
inline void insert(const KEY& key, const KEY& parent) {
std::map<KEY, KEY>::insert(std::make_pair(key, parent));
}
};
/**
* Generate a list of keys from a spanning tree represented by its predecessor map
*/
template<class Key>
std::list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map);
template<class KEY>
std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map);
/**
* Convert the factor graph to an SDGraph
@ -74,36 +74,36 @@ namespace gtsam {
* F = Factor type
* Key = Key type
*/
template<class G, class F, class Key> SDGraph<Key> toBoostGraph(const G& graph);
template<class G, class F, class KEY> SDGraph<KEY> toBoostGraph(const G& graph);
/**
* Build takes a predecessor map, and builds a directed graph corresponding to the tree.
* G = Graph type
* V = Vertex type
*/
template<class G, class V, class Key>
boost::tuple<G, V, std::map<Key,V> > predecessorMap2Graph(const PredecessorMap<Key>& p_map);
template<class G, class V, class KEY>
boost::tuple<G, V, std::map<KEY,V> > predecessorMap2Graph(const PredecessorMap<KEY>& p_map);
/**
* Compose the poses by following the chain specified by the spanning tree
*/
template<class G, class Factor, class Pose, class Values>
boost::shared_ptr<Values>
composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree, const Pose& rootPose);
template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<VALUES>
composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree, const POSE& rootPose);
/**
* find the minimum spanning tree using boost graph library
*/
template<class G, class Key, class Factor2>
PredecessorMap<Key> findMinimumSpanningTree(const G& g) ;
template<class G, class KEY, class FACTOR2>
PredecessorMap<KEY> findMinimumSpanningTree(const G& g) ;
/**
* Split the graph into two parts: one corresponds to the given spanning tree,
* and the other corresponds to the rest of the factors
*/
template<class G, class Key, class Factor2>
void split(const G& g, const PredecessorMap<Key>& tree, G& Ab1, G& Ab2) ;
template<class G, class KEY, class FACTOR2>
void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) ;
} // namespace gtsam

82
inference/inference-inl.h
View File

@ -40,12 +40,12 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class FactorGraph>
inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(const FactorGraph& factorGraph) {
template<class FACTORGRAPH>
inline typename FACTORGRAPH::bayesnet_type::shared_ptr Inference::Eliminate(const FACTORGRAPH& factorGraph) {
// Create a copy of the factor graph to eliminate in-place
FactorGraph eliminationGraph(factorGraph);
typename FactorGraph::variableindex_type variableIndex(eliminationGraph);
FACTORGRAPH eliminationGraph(factorGraph);
typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph);
return Eliminate(eliminationGraph, variableIndex);
}
@ -72,36 +72,36 @@ inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(cons
//}
/* ************************************************************************* */
template<class FactorGraph>
inline typename FactorGraph::bayesnet_type::shared_ptr
Inference::Eliminate(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex) {
template<class FACTORGRAPH>
inline typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::Eliminate(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex) {
return EliminateUntil(factorGraph, variableIndex.size(), variableIndex);
}
/* ************************************************************************* */
template<class FactorGraph>
inline typename FactorGraph::bayesnet_type::shared_ptr
Inference::EliminateUntil(const FactorGraph& factorGraph, Index bound) {
template<class FACTORGRAPH>
inline typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::EliminateUntil(const FACTORGRAPH& factorGraph, Index bound) {
// Create a copy of the factor graph to eliminate in-place
FactorGraph eliminationGraph(factorGraph);
typename FactorGraph::variableindex_type variableIndex(eliminationGraph);
FACTORGRAPH eliminationGraph(factorGraph);
typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph);
return EliminateUntil(eliminationGraph, bound, variableIndex);
}
/* ************************************************************************* */
template<class FactorGraph>
typename FactorGraph::bayesnet_type::shared_ptr
Inference::EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex) {
template<class FACTORGRAPH>
typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex) {
typename FactorGraph::bayesnet_type::shared_ptr bayesnet(new typename FactorGraph::bayesnet_type);
typename FACTORGRAPH::bayesnet_type::shared_ptr bayesnet(new typename FACTORGRAPH::bayesnet_type);
// Eliminate variables one-by-one, updating the eliminated factor graph and
// the variable index.
for(Index var = 0; var < bound; ++var) {
typename FactorGraph::bayesnet_type::sharedConditional conditional(EliminateOne(factorGraph, variableIndex, var));
typename FACTORGRAPH::bayesnet_type::sharedConditional conditional(EliminateOne(factorGraph, variableIndex, var));
if(conditional) // Will be NULL if the variable did not appear in the factor graph.
bayesnet->push_back(conditional);
}
@ -110,9 +110,9 @@ Inference::EliminateUntil(FactorGraph& factorGraph, Index bound, typename Factor
}
/* ************************************************************************* */
template<class FactorGraph>
typename FactorGraph::bayesnet_type::sharedConditional
Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var) {
template<class FACTORGRAPH>
typename FACTORGRAPH::bayesnet_type::sharedConditional
Inference::EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var) {
/* This function performs symbolic elimination of a variable, comprising
* combining involved factors (analogous to "assembly" in SPQR) followed by
@ -140,14 +140,14 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
tic("EliminateOne");
// Get the factors involving the eliminated variable
typename FactorGraph::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]);
typedef typename FactorGraph::variableindex_type::mapped_factor_type mapped_factor_type;
typename FACTORGRAPH::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]);
typedef typename FACTORGRAPH::variableindex_type::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(&FactorGraph::variableindex_type::mapped_factor_type::factorIndex, boost::lambda::_1));
boost::lambda::bind(&FACTORGRAPH::variableindex_type::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();
@ -262,8 +262,8 @@ 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(
typename FACTORGRAPH::sharedFactor jointFactor(
FACTORGRAPH::Factor::Combine(
factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions));
toc("EliminateOne: Combine");
@ -273,7 +273,7 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
factorGraph.remove(removedFactorI);
}
typename FactorGraph::bayesnet_type::sharedConditional conditional;
typename FACTORGRAPH::bayesnet_type::sharedConditional conditional;
tic("EliminateOne: eliminateFirst");
conditional = jointFactor->eliminateFirst(); // Eliminate the first variable in-place
toc("EliminateOne: eliminateFirst");
@ -288,38 +288,38 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
} else { // varIndexEntry.empty()
toc("EliminateOne");
return typename FactorGraph::bayesnet_type::sharedConditional();
return typename FACTORGRAPH::bayesnet_type::sharedConditional();
}
}
/* ************************************************************************* */
template<class FactorGraph, class VarContainer>
FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContainer& variables) {
template<class FACTORGRAPH, class VARCONTAINER>
FACTORGRAPH Inference::Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables) {
// Compute a COLAMD permutation with the marginal variables constrained to the end
typename FactorGraph::variableindex_type varIndex(factorGraph);
typename FACTORGRAPH::variableindex_type varIndex(factorGraph);
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(varIndex, variables));
Permutation::shared_ptr permutationInverse(permutation->inverse());
// Copy and permute the factors
varIndex.permute(*permutation);
FactorGraph eliminationGraph; eliminationGraph.reserve(factorGraph.size());
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) {
typename FactorGraph::sharedFactor permFactor(new typename FactorGraph::Factor(*factor));
FACTORGRAPH eliminationGraph; eliminationGraph.reserve(factorGraph.size());
BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, factorGraph) {
typename FACTORGRAPH::sharedFactor permFactor(new typename FACTORGRAPH::Factor(*factor));
permFactor->permuteWithInverse(*permutationInverse);
eliminationGraph.push_back(permFactor);
}
// Eliminate all variables
typename FactorGraph::bayesnet_type::shared_ptr bn(Inference::Eliminate(eliminationGraph, varIndex));
typename FACTORGRAPH::bayesnet_type::shared_ptr bn(Inference::Eliminate(eliminationGraph, varIndex));
// The last conditionals in the eliminated BayesNet contain the marginal for
// the variables we want. Undo the permutation as we add the marginal
// factors.
FactorGraph marginal; marginal.reserve(variables.size());
typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin();
FACTORGRAPH marginal; marginal.reserve(variables.size());
typename FACTORGRAPH::bayesnet_type::const_reverse_iterator conditional = bn->rbegin();
for(Index j=0; j<variables.size(); ++j, ++conditional) {
typename FactorGraph::sharedFactor factor(new typename FactorGraph::Factor(**conditional));
typename FACTORGRAPH::sharedFactor factor(new typename FACTORGRAPH::Factor(**conditional));
factor->permuteWithInverse(*permutation);
marginal.push_back(factor);
assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end());
@ -330,8 +330,8 @@ FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContain
}
/* ************************************************************************* */
template<class VariableIndexType, typename ConstraintContainer>
Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast) {
template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast) {
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!)
int Alen = ccolamd_recommended(nEntries, nFactors, nVars); /* colamd arg 3: size of the array A */
@ -344,9 +344,9 @@ Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& va
p[0] = 0;
int count = 0;
for(Index var = 0; var < variableIndex.size(); ++var) {
const typename VariableIndexType::mapped_type& column(variableIndex[var]);
const typename VARIABLEINDEXTYPE::mapped_type& column(variableIndex[var]);
size_t lastFactorId = numeric_limits<size_t>::max();
BOOST_FOREACH(const typename VariableIndexType::mapped_factor_type& factor_pos, column) {
BOOST_FOREACH(const typename VARIABLEINDEXTYPE::mapped_factor_type& factor_pos, column) {
if(lastFactorId != numeric_limits<size_t>::max())
assert(factor_pos.factorIndex > lastFactorId);
A[count++] = factor_pos.factorIndex; // copy sparse column

44
inference/inference.h
View File

@ -40,8 +40,8 @@ namespace gtsam {
* Eliminate a factor graph in its natural ordering, i.e. eliminating
* variables in order starting from 0.
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::shared_ptr Eliminate(const FactorGraph& factorGraph);
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(const FACTORGRAPH& factorGraph);
/**
* Eliminate a factor graph in its natural ordering, i.e. eliminating
@ -56,34 +56,34 @@ namespace gtsam {
* variables in order starting from 0. Uses an existing
* variable index instead of building one from scratch.
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::shared_ptr Eliminate(
FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex);
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(
FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex);
/**
* Partially eliminate a factor graph, up to but not including the given
* variable.
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::shared_ptr
EliminateUntil(const FactorGraph& factorGraph, Index bound);
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr
EliminateUntil(const FACTORGRAPH& factorGraph, Index bound);
/**
* Partially eliminate a factor graph, up to but not including the given
* variable. Use an existing variable index instead of building one from
* scratch.
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::shared_ptr
EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex);
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::shared_ptr
EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex);
/**
* Eliminate a single variable, updating an existing factor graph and
* variable index.
*/
template<class FactorGraph>
static typename FactorGraph::bayesnet_type::sharedConditional
EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var);
template<class FACTORGRAPH>
static typename FACTORGRAPH::bayesnet_type::sharedConditional
EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var);
/**
* Eliminate a single variable, updating an existing factor graph and
@ -101,24 +101,24 @@ namespace gtsam {
* BayesTree which supports efficiently computing marginals for multiple
* variables.
*/
template<class FactorGraph, class VarContainer>
static FactorGraph Marginal(const FactorGraph& factorGraph, const VarContainer& variables);
template<class FACTORGRAPH, class VARCONTAINER>
static FACTORGRAPH Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables);
/**
* Compute a permutation (variable ordering) using colamd
*/
template<class VariableIndexType>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VariableIndexType& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector<Index>()); }
template<class VariableIndexType, typename ConstraintContainer>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast);
template<class VARIABLEINDEXTYPE>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector<Index>()); }
template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast);
// /**
// * Join several factors into one. This involves determining the set of
// * shared variables and the correct variable positions in the new joint
// * factor.
// */
// template<class FactorGraph, typename InputIterator>
// static typename FactorGraph::shared_factor Combine(const FactorGraph& factorGraph,
// template<class FACTORGRAPH, typename InputIterator>
// static typename FACTORGRAPH::shared_factor Combine(const FACTORGRAPH& factorGraph,
// InputIterator indicesBegin, InputIterator indicesEnd);