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: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class ARCHIVE>
void serialize(Archive & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(conditionals_); ar & BOOST_SERIALIZATION_NVP(conditionals_);
} }
}; // BayesNet }; // BayesNet

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? // TODO, why do we actually return a shared pointer, why does eliminate?
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
template<class FactorGraph> template<class FACTORGRAPH>
BayesNet<CONDITIONAL> BayesNet<CONDITIONAL>
BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R) { BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R) {
// A first base case is when this clique or its parent is the root, // 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 // The parent clique has a CONDITIONAL for each frontal node in Fp
// so we can obtain P(Fp|Sp) in factor graph form // 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 // 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) // 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 // Eliminate into a Bayes net with ordering designed to integrate out
// any variables not in *our* separator. Variables to integrate out must be // 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); BOOST_FOREACH(Index key, separator) ordering.push_back(key);
// eliminate to get marginal // 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 toFront = Permutation::PullToFront(ordering, varIndex.size());
Permutation::shared_ptr toFrontInverse(toFront.inverse()); 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); factor->permuteWithInverse(*toFrontInverse);
} }
varIndex.permute(toFront); varIndex.permute(toFront);
@ -346,19 +346,18 @@ namespace gtsam {
// Because the root clique could be very big. // Because the root clique could be very big.
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
template<class FactorGraph> template<class FACTORGRAPH>
FactorGraph FACTORGRAPH BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
// If we are the root, just return this root // If we are the root, just return this root
if (R.get()==this) return *R; if (R.get()==this) return *R;
// Combine P(F|S), P(S|R), and P(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_front(*this);
p_FSR.push_back(*R); p_FSR.push_back(*R);
// Find marginal on the keys we are interested in // 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 CONDITIONAL>
template<class Container> template<class CONTAINER>
inline Index BayesTree<CONDITIONAL>::findParentClique(const Container& parents) const { inline Index BayesTree<CONDITIONAL>::findParentClique(const CONTAINER& parents) const {
typename Container::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end()); typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
assert(lowestOrderedParent != parents.end()); assert(lowestOrderedParent != parents.end());
return *lowestOrderedParent; return *lowestOrderedParent;
@ -690,29 +689,28 @@ namespace gtsam {
// First finds clique marginal then marginalizes that // First finds clique marginal then marginalizes that
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
template<class FactorGraph> template<class FACTORGRAPH>
FactorGraph FACTORGRAPH BayesTree<CONDITIONAL>::marginal(Index key) const {
BayesTree<CONDITIONAL>::marginal(Index key) const {
// get clique containing key // get clique containing key
sharedClique clique = (*this)[key]; sharedClique clique = (*this)[key];
// calculate or retrieve its marginal // 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 // 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; vector<Index> keyAsVector(1); keyAsVector[0] = key;
Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size())); Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size()));
Permutation::shared_ptr toBackInverse(toBack.inverse()); Permutation::shared_ptr toBackInverse(toBack.inverse());
varIndex.permute(toBack); varIndex.permute(toBack);
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, cliqueMarginal) { BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
factor->permuteWithInverse(*toBackInverse); factor->permuteWithInverse(*toBackInverse);
} }
// partially eliminate, remaining factor graph is requested marginal // partially eliminate, remaining factor graph is requested marginal
Inference::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex); 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) if(factor)
factor->permuteWithInverse(toBack); factor->permuteWithInverse(toBack);
} }
@ -721,12 +719,11 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
template<class FactorGraph> template<class FACTORGRAPH>
BayesNet<CONDITIONAL> BayesNet<CONDITIONAL> BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
// calculate marginal as a factor graph // calculate marginal as a factor graph
FactorGraph fg = this->marginal<FactorGraph>(key); FACTORGRAPH fg = this->marginal<FACTORGRAPH>(key);
// eliminate further to Bayes net // eliminate further to Bayes net
return *Inference::Eliminate(fg); return *Inference::Eliminate(fg);
@ -808,8 +805,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template<class CONDITIONAL>
template<class Container> template<class CONTAINER>
void BayesTree<CONDITIONAL>::removeTop(const Container& keys, void BayesTree<CONDITIONAL>::removeTop(const CONTAINER& keys,
BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) { BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) {
// process each key of the new factor // 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 */ /** return the conditional P(S|Root) on the separator given the root */
// TODO: create a cached version // TODO: create a cached version
template<class FactorGraph> template<class FACTORGRAPH>
BayesNet<CONDITIONAL> shortcut(shared_ptr root); BayesNet<CONDITIONAL> shortcut(shared_ptr root);
/** return the marginal P(C) of the clique */ /** return the marginal P(C) of the clique */
template<class FactorGraph> template<class FACTORGRAPH>
FactorGraph marginal(shared_ptr root); FACTORGRAPH marginal(shared_ptr root);
// /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ // /** return the joint P(C1,C2), where C1==this. TODO: not a method? */
// template<class Factor> // template<class Factor>
@ -233,8 +233,8 @@ namespace gtsam {
* Find parent clique of a conditional. It will look at all parents and * Find parent clique of a conditional. It will look at all parents and
* return the one with the lowest index in the ordering. * return the one with the lowest index in the ordering.
*/ */
template<class Container> template<class CONTAINER>
Index findParentClique(const Container& parents) const; Index findParentClique(const CONTAINER& parents) const;
/** number of cliques */ /** number of cliques */
inline size_t size() const { inline size_t size() const {
@ -257,11 +257,11 @@ namespace gtsam {
CliqueData getCliqueData() const; CliqueData getCliqueData() const;
/** return marginal on any variable */ /** return marginal on any variable */
template<class FactorGraph> template<class FACTORGRAPH>
FactorGraph marginal(Index key) const; FACTORGRAPH marginal(Index key) const;
/** return marginal on any variable, as a Bayes Net */ /** return marginal on any variable, as a Bayes Net */
template<class FactorGraph> template<class FACTORGRAPH>
BayesNet<CONDITIONAL> marginalBayesNet(Index key) const; BayesNet<CONDITIONAL> marginalBayesNet(Index key) const;
// /** return joint on two variables */ // /** 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. * 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. * Factors and orphans are added to the in/out arguments.
*/ */
template<class Container> template<class CONTAINER>
void removeTop(const Container& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans); void removeTop(const CONTAINER& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
}; // BayesTree }; // BayesTree

8
inference/ClusterTree-inl.h
View File

@ -37,16 +37,16 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
template<typename FrontalIt, typename SeparatorIt> template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster( 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) {} FG(fg), frontal(firstFrontal, lastFrontal), separator(firstSeparator, lastSeparator) {}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
template<typename FrontalIt, typename SeparatorIt> template<typename FRONTALIT, typename SEPARATORIT>
ClusterTree<FG>::Cluster::Cluster( 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) {} 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); Cluster(const FG& fg, Index key, Iterator firstSeparator, Iterator lastSeparator);
/* Create a node with several frontal variables */ /* Create a node with several frontal variables */
template<typename FrontalIt, typename SeparatorIt> template<typename FRONTALIT, typename SEPARATORIT>
Cluster(const FG& fg, FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator); Cluster(const FG& fg, FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
/* Create a node with several frontal variables */ /* Create a node with several frontal variables */
template<typename FrontalIt, typename SeparatorIt> template<typename FRONTALIT, typename SEPARATORIT>
Cluster(FrontalIt firstFrontal, FrontalIt lastFrontal, SeparatorIt firstSeparator, SeparatorIt lastSeparator); Cluster(FRONTALIT firstFrontal, FRONTALIT lastFrontal, SEPARATORIT firstSeparator, SEPARATORIT lastSeparator);
// print the object // print the object
void print(const std::string& indent) const; void print(const std::string& indent) const;

4
inference/Conditional.h
View File

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

8
inference/Factor.h
View File

@ -77,7 +77,7 @@ public:
FactorBase(const Conditional& c); FactorBase(const Conditional& c);
/** Constructor from a collection of keys */ /** 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(); } keys_(beginKey, endKey) { assertInvariants(); }
/** Default constructor for I/O */ /** Default constructor for I/O */
@ -102,9 +102,9 @@ public:
/** Named constructor for combining a set of factors with pre-computed set of /** Named constructor for combining a set of factors with pre-computed set of
* variables. (Old style - will be removed when scalar elimination is * variables. (Old style - will be removed when scalar elimination is
* removed in favor of the EliminationTree). */ * removed in favor of the EliminationTree). */
template<class DERIVED, class FactorGraphType, class VariableIndexStorage> template<class DERIVED, class FACTORGRAPHTYPE, class VARIABLEINDEXSTORAGE>
static typename DERIVED::shared_ptr Combine(const FactorGraphType& factorGraph, static typename DERIVED::shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph,
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors, const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const std::vector<size_t>& factors,
const std::vector<KEY>& 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())); } 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> template<class FACTORGRAPH>
FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) { FACTORGRAPH combine(const FACTORGRAPH& fg1, const FACTORGRAPH& fg2) {
// create new linear factor graph equal to the first one // create new linear factor graph equal to the first one
FactorGraph fg = fg1; FACTORGRAPH fg = fg1;
// add the second factors_ in the graph // add the second factors_ in the graph
fg.push_back(fg2); fg.push_back(fg2);

46
inference/FactorGraph.h
View File

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

30
inference/ISAM-inl.h
View File

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

18
inference/ISAM.h
View File

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

6
inference/IndexFactor.h
View File

@ -63,9 +63,9 @@ public:
static shared_ptr static shared_ptr
Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots); Combine(const FactorGraph<This>& factors, const FastMap<Index, std::vector<Index> >& variableSlots);
template<class FactorGraphType, class VariableIndexStorage> template<class FACTORGRAPHTYPE, class VARIABLEINDEXSTORAGE>
static shared_ptr Combine(const FactorGraphType& factorGraph, static shared_ptr Combine(const FACTORGRAPHTYPE& factorGraph,
const VariableIndex<VariableIndexStorage>& variableIndex, const std::vector<size_t>& factors, const VariableIndex<VARIABLEINDEXSTORAGE>& variableIndex, const std::vector<size_t>& factors,
const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) { const std::vector<Index>& variables, const std::vector<std::vector<size_t> >& variablePositions) {
return Base::Combine<This>(factorGraph, variableIndex, factors, variables, 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. * 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 { class Permuted {
Permutation permutation_; Permutation permutation_;
Container& container_; CONTAINER& container_;
public: public:
typedef ValueType value_type; typedef VALUETYPE value_type;
/** Construct as a permuted view on the Container. The permutation is copied /** Construct as a permuted view on the Container. The permutation is copied
* but only a reference to the container is stored. * 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 /** Construct as a view on the Container with an identity permutation. Only
* a reference to the container is stored. * 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 */ /** Access the container through the permutation */
value_type operator[](size_t index) const { return container_[permutation_[index]]; } 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); } void permute(const Permutation& permutation) { assert(permutation.size() == this->size()); permutation_ = *permutation_.permute(permutation); }
/** Access the underlying container */ /** Access the underlying container */
Container* operator->() { return &container_; } CONTAINER* operator->() { return &container_; }
/** Access the underlying container (const version) */ /** Access the underlying container (const version) */
const Container* operator->() const { return &container_; } const CONTAINER* operator->() const { return &container_; }
/** Size of the underlying container */ /** Size of the underlying container */
size_t size() const { return container_.size(); } size_t size() const { return container_.size(); }
/** Access to the underlying container */ /** Access to the underlying container */
Container& container() { return container_; } CONTAINER& container() { return container_; }
/** Access to the underlying container (const version) */ /** Access to the underlying container (const version) */
const Container& container() const { return container_; } const CONTAINER& container() const { return container_; }
/** Access the underlying permutation */ /** Access the underlying permutation */
Permutation& permutation() { return 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) {} _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; } 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 { class VariableIndex {
public: public:
@ -64,7 +64,7 @@ public:
typedef typename mapped_type::const_iterator const_factor_iterator; typedef typename mapped_type::const_iterator const_factor_iterator;
protected: 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_; storage_type indexUnpermuted_;
Permuted<storage_type, typename storage_type::value_type&> index_; Permuted<storage_type, typename storage_type::value_type&> index_;
size_t nFactors_; size_t nFactors_;

118
inference/graph-inl.h
View File

@ -30,42 +30,42 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template <class Key> template <class KEY>
class ordering_key_visitor : public boost::default_bfs_visitor { class ordering_key_visitor : public boost::default_bfs_visitor {
public: 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 { 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); ordering_.push_front(key);
} }
std::list<Key>& ordering_; std::list<KEY>& ordering_;
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<class Key> template<class KEY>
list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map) { 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; SVertex root;
std::map<Key, SVertex> key2vertex; std::map<KEY, SVertex> key2vertex;
boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map); boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
// breadth first visit on the graph // breadth first visit on the graph
std::list<Key> keys; std::list<KEY> keys;
ordering_key_visitor<Key> vis(keys); ordering_key_visitor<KEY> vis(keys);
boost::breadth_first_search(g, root, boost::visitor(vis)); boost::breadth_first_search(g, root, boost::visitor(vis));
return keys; return keys;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class F, class Key> template<class G, class F, class KEY>
SDGraph<Key> toBoostGraph(const G& graph) { SDGraph<KEY> toBoostGraph(const G& graph) {
// convert the factor graph to boost graph // convert the factor graph to boost graph
SDGraph<Key> g; SDGraph<KEY> g;
typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex; typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
map<Key, BoostVertex> key2vertex; map<KEY, BoostVertex> key2vertex;
BoostVertex v1, v2; BoostVertex v1, v2;
typename G::const_iterator itFactor; 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); boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
if (!factor) continue; if (!factor) continue;
Key key1 = factor->key1(); KEY key1 = factor->key1();
Key key2 = factor->key2(); KEY key2 = factor->key2();
if (key2vertex.find(key1) == key2vertex.end()) { if (key2vertex.find(key1) == key2vertex.end()) {
v1 = add_vertex(key1, g); v1 = add_vertex(key1, g);
@ -102,14 +102,14 @@ SDGraph<Key> toBoostGraph(const G& graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class V, class Key> template<class G, class V, class KEY>
boost::tuple<G, V, map<Key,V> > boost::tuple<G, V, map<KEY,V> >
predecessorMap2Graph(const PredecessorMap<Key>& p_map) { predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
G g; G g;
map<Key, V> key2vertex; map<KEY, V> key2vertex;
V v1, v2, root; V v1, v2, root;
Key child, parent; KEY child, parent;
bool foundRoot = false; bool foundRoot = false;
FOREACH_PAIR(child, parent, p_map) { FOREACH_PAIR(child, parent, p_map) {
if (key2vertex.find(child) == key2vertex.end()) { if (key2vertex.find(child) == key2vertex.end()) {
@ -134,47 +134,47 @@ predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
if (!foundRoot) if (!foundRoot)
throw invalid_argument("predecessorMap2Graph: invalid predecessor map!"); throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
else 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 { class compose_key_visitor : public boost::default_bfs_visitor {
private: private:
boost::shared_ptr<Values> config_; boost::shared_ptr<VALUES> config_;
public: 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 { 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_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)); typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
Pose relativePose = boost::get(boost::edge_weight, g, edge); POSE relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, (*config_)[key_from].compose(relativePose)); config_->insert(key_to, (*config_)[key_from].compose(relativePose));
} }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class Factor, class Pose, class Values> template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree, boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
const Pose& rootPose) { const POSE& rootPose) {
//TODO: change edge_weight_t to edge_pose_t //TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list< typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS, boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, typename Values::Key>, boost::property<boost::vertex_name_t, typename VALUES::Key>,
boost::property<boost::edge_weight_t, Pose> > PoseGraph; boost::property<boost::edge_weight_t, POSE> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex; typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge; typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g; PoseGraph g;
PoseVertex root; PoseVertex root;
map<typename Values::Key, PoseVertex> key2vertex; map<typename VALUES::Key, PoseVertex> key2vertex;
boost::tie(g, root, 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 // attach the relative poses to the edges
PoseEdge edge12, edge21; 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); boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue; if (!factor) continue;
typename Values::Key key1 = factor->key1(); typename VALUES::Key key1 = factor->key1();
typename Values::Key key2 = factor->key2(); typename VALUES::Key key2 = factor->key2();
PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second; PoseVertex v2 = key2vertex.find(key2)->second;
Pose l1Xl2 = factor->measured(); POSE l1Xl2 = factor->measured();
tie(edge12, found1) = boost::edge(v1, v2, g); tie(edge12, found1) = boost::edge(v1, v2, g);
tie(edge21, found2) = boost::edge(v2, v1, g); tie(edge21, found2) = boost::edge(v2, v1, g);
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree"); 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 // compose poses
boost::shared_ptr<Values> config(new Values); boost::shared_ptr<VALUES> config(new VALUES);
typename Values::Key rootKey = boost::get(boost::vertex_name, g, root); typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose); 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)); boost::breadth_first_search(g, root, boost::visitor(vis));
return config; return config;
@ -218,22 +218,22 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<type
/* ************************************************************************* */ /* ************************************************************************* */
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class Key, class Factor2> template<class G, class KEY, class FACTOR2>
PredecessorMap<Key> findMinimumSpanningTree(const G& fg) { 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 // 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]); prim_minimum_spanning_tree(g, &p_map[0]);
// convert edge to string pairs // convert edge to string pairs
PredecessorMap<Key> tree; PredecessorMap<KEY> tree;
typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first; typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
typename vector<typename SDGraph<Key>::Vertex>::iterator vi; typename vector<typename SDGraph<KEY>::Vertex>::iterator vi;
for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) { for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) {
Key key = boost::get(boost::vertex_name, g, *itVertex); KEY key = boost::get(boost::vertex_name, g, *itVertex);
Key parent = boost::get(boost::vertex_name, g, *vi); KEY parent = boost::get(boost::vertex_name, g, *vi);
tree.insert(key, parent); tree.insert(key, parent);
} }
@ -241,8 +241,8 @@ PredecessorMap<Key> findMinimumSpanningTree(const G& fg) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class Key, class Factor2> template<class G, class KEY, class FACTOR2>
void split(const G& g, const PredecessorMap<Key>& tree, G& Ab1, G& Ab2) { void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
typedef typename G::sharedFactor F ; typedef typename G::sharedFactor F ;
@ -256,12 +256,12 @@ void split(const G& g, const PredecessorMap<Key>& tree, G& Ab1, G& Ab2) {
continue; continue;
} }
boost::shared_ptr<Factor2> factor2 = boost::dynamic_pointer_cast< boost::shared_ptr<FACTOR2> factor2 = boost::dynamic_pointer_cast<
Factor2>(factor); FACTOR2>(factor);
if (!factor2) continue; if (!factor2) continue;
Key key1 = factor2->key1(); KEY key1 = factor2->key1();
Key key2 = factor2->key2(); KEY key2 = factor2->key2();
// if the tree contains the key // if the tree contains the key
if ((tree.find(key1) != tree.end() && if ((tree.find(key1) != tree.end() &&
tree.find(key1)->second.compare(key2) == 0) || 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 * 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, 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> > { boost::edge_weight_t, double> > {
public: 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, 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: 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; //typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
@ -53,20 +53,20 @@ namespace gtsam {
/** /**
* Map from variable key to parent key * Map from variable key to parent key
*/ */
template<class Key> template<class KEY>
class PredecessorMap: public std::map<Key, Key> { class PredecessorMap: public std::map<KEY, KEY> {
public: public:
/** convenience insert so we can pass ints for TypedSymbol keys */ /** convenience insert so we can pass ints for TypedSymbol keys */
inline void insert(const Key& key, const Key& parent) { inline void insert(const KEY& key, const KEY& parent) {
std::map<Key, Key>::insert(std::make_pair(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 * Generate a list of keys from a spanning tree represented by its predecessor map
*/ */
template<class Key> template<class KEY>
std::list<Key> predecessorMap2Keys(const PredecessorMap<Key>& p_map); std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map);
/** /**
* Convert the factor graph to an SDGraph * Convert the factor graph to an SDGraph
@ -74,36 +74,36 @@ namespace gtsam {
* F = Factor type * F = Factor type
* Key = Key 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. * Build takes a predecessor map, and builds a directed graph corresponding to the tree.
* G = Graph type * G = Graph type
* V = Vertex type * V = Vertex type
*/ */
template<class G, class V, class Key> template<class G, class V, class KEY>
boost::tuple<G, V, std::map<Key,V> > predecessorMap2Graph(const PredecessorMap<Key>& p_map); 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 * Compose the poses by following the chain specified by the spanning tree
*/ */
template<class G, class Factor, class Pose, class Values> template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<Values> boost::shared_ptr<VALUES>
composePoses(const G& graph, const PredecessorMap<typename Values::Key>& tree, const Pose& rootPose); composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree, const POSE& rootPose);
/** /**
* find the minimum spanning tree using boost graph library * find the minimum spanning tree using boost graph library
*/ */
template<class G, class Key, class Factor2> template<class G, class KEY, class FACTOR2>
PredecessorMap<Key> findMinimumSpanningTree(const G& g) ; PredecessorMap<KEY> findMinimumSpanningTree(const G& g) ;
/** /**
* Split the graph into two parts: one corresponds to the given spanning tree, * Split the graph into two parts: one corresponds to the given spanning tree,
* and the other corresponds to the rest of the factors * and the other corresponds to the rest of the factors
*/ */
template<class G, class Key, class Factor2> template<class G, class KEY, class FACTOR2>
void split(const G& g, const PredecessorMap<Key>& tree, G& Ab1, G& Ab2) ; void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) ;
} // namespace gtsam } // namespace gtsam

82
inference/inference-inl.h
View File

@ -40,12 +40,12 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph> template<class FACTORGRAPH>
inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(const FactorGraph& factorGraph) { inline typename FACTORGRAPH::bayesnet_type::shared_ptr Inference::Eliminate(const FACTORGRAPH& factorGraph) {
// Create a copy of the factor graph to eliminate in-place // Create a copy of the factor graph to eliminate in-place
FactorGraph eliminationGraph(factorGraph); FACTORGRAPH eliminationGraph(factorGraph);
typename FactorGraph::variableindex_type variableIndex(eliminationGraph); typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph);
return Eliminate(eliminationGraph, variableIndex); return Eliminate(eliminationGraph, variableIndex);
} }
@ -72,36 +72,36 @@ inline typename FactorGraph::bayesnet_type::shared_ptr Inference::Eliminate(cons
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph> template<class FACTORGRAPH>
inline typename FactorGraph::bayesnet_type::shared_ptr inline typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::Eliminate(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex) { Inference::Eliminate(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex) {
return EliminateUntil(factorGraph, variableIndex.size(), variableIndex); return EliminateUntil(factorGraph, variableIndex.size(), variableIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph> template<class FACTORGRAPH>
inline typename FactorGraph::bayesnet_type::shared_ptr inline typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::EliminateUntil(const FactorGraph& factorGraph, Index bound) { Inference::EliminateUntil(const FACTORGRAPH& factorGraph, Index bound) {
// Create a copy of the factor graph to eliminate in-place // Create a copy of the factor graph to eliminate in-place
FactorGraph eliminationGraph(factorGraph); FACTORGRAPH eliminationGraph(factorGraph);
typename FactorGraph::variableindex_type variableIndex(eliminationGraph); typename FACTORGRAPH::variableindex_type variableIndex(eliminationGraph);
return EliminateUntil(eliminationGraph, bound, variableIndex); return EliminateUntil(eliminationGraph, bound, variableIndex);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph> template<class FACTORGRAPH>
typename FactorGraph::bayesnet_type::shared_ptr typename FACTORGRAPH::bayesnet_type::shared_ptr
Inference::EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex) { 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 // Eliminate variables one-by-one, updating the eliminated factor graph and
// the variable index. // the variable index.
for(Index var = 0; var < bound; ++var) { 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. if(conditional) // Will be NULL if the variable did not appear in the factor graph.
bayesnet->push_back(conditional); bayesnet->push_back(conditional);
} }
@ -110,9 +110,9 @@ Inference::EliminateUntil(FactorGraph& factorGraph, Index bound, typename Factor
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph> template<class FACTORGRAPH>
typename FactorGraph::bayesnet_type::sharedConditional typename FACTORGRAPH::bayesnet_type::sharedConditional
Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var) { Inference::EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var) {
/* This function performs symbolic elimination of a variable, comprising /* This function performs symbolic elimination of a variable, comprising
* combining involved factors (analogous to "assembly" in SPQR) followed by * combining involved factors (analogous to "assembly" in SPQR) followed by
@ -140,14 +140,14 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
tic("EliminateOne"); tic("EliminateOne");
// Get the factors involving the eliminated variable // Get the factors involving the eliminated variable
typename FactorGraph::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]); typename FACTORGRAPH::variableindex_type::mapped_type& varIndexEntry(variableIndex[var]);
typedef typename FactorGraph::variableindex_type::mapped_factor_type mapped_factor_type; typedef typename FACTORGRAPH::variableindex_type::mapped_factor_type mapped_factor_type;
if(!varIndexEntry.empty()) { if(!varIndexEntry.empty()) {
vector<size_t> removedFactors(varIndexEntry.size()); vector<size_t> removedFactors(varIndexEntry.size());
transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(), transform(varIndexEntry.begin(), varIndexEntry.end(), removedFactors.begin(),
boost::lambda::bind(&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 // The new joint factor will be the last one in the factor graph
size_t jointFactorIndex = factorGraph.size(); 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 // Join the factors and eliminate the variable from the joint factor
tic("EliminateOne: Combine"); tic("EliminateOne: Combine");
typename FactorGraph::sharedFactor jointFactor( typename FACTORGRAPH::sharedFactor jointFactor(
FactorGraph::Factor::Combine( FACTORGRAPH::Factor::Combine(
factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions)); factorGraph, variableIndex, removedFactorIdxs, sortedKeys, jointFactorPositions));
toc("EliminateOne: Combine"); toc("EliminateOne: Combine");
@ -273,7 +273,7 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
factorGraph.remove(removedFactorI); factorGraph.remove(removedFactorI);
} }
typename FactorGraph::bayesnet_type::sharedConditional conditional; typename FACTORGRAPH::bayesnet_type::sharedConditional conditional;
tic("EliminateOne: eliminateFirst"); tic("EliminateOne: eliminateFirst");
conditional = jointFactor->eliminateFirst(); // Eliminate the first variable in-place conditional = jointFactor->eliminateFirst(); // Eliminate the first variable in-place
toc("EliminateOne: eliminateFirst"); toc("EliminateOne: eliminateFirst");
@ -288,38 +288,38 @@ Inference::EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variable
} else { // varIndexEntry.empty() } else { // varIndexEntry.empty()
toc("EliminateOne"); toc("EliminateOne");
return typename FactorGraph::bayesnet_type::sharedConditional(); return typename FACTORGRAPH::bayesnet_type::sharedConditional();
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FactorGraph, class VarContainer> template<class FACTORGRAPH, class VARCONTAINER>
FactorGraph Inference::Marginal(const FactorGraph& factorGraph, const VarContainer& variables) { FACTORGRAPH Inference::Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables) {
// Compute a COLAMD permutation with the marginal variables constrained to the end // 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 permutation(Inference::PermutationCOLAMD(varIndex, variables));
Permutation::shared_ptr permutationInverse(permutation->inverse()); Permutation::shared_ptr permutationInverse(permutation->inverse());
// Copy and permute the factors // Copy and permute the factors
varIndex.permute(*permutation); varIndex.permute(*permutation);
FactorGraph eliminationGraph; eliminationGraph.reserve(factorGraph.size()); FACTORGRAPH eliminationGraph; eliminationGraph.reserve(factorGraph.size());
BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) { BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, factorGraph) {
typename FactorGraph::sharedFactor permFactor(new typename FactorGraph::Factor(*factor)); typename FACTORGRAPH::sharedFactor permFactor(new typename FACTORGRAPH::Factor(*factor));
permFactor->permuteWithInverse(*permutationInverse); permFactor->permuteWithInverse(*permutationInverse);
eliminationGraph.push_back(permFactor); eliminationGraph.push_back(permFactor);
} }
// Eliminate all variables // 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 last conditionals in the eliminated BayesNet contain the marginal for
// the variables we want. Undo the permutation as we add the marginal // the variables we want. Undo the permutation as we add the marginal
// factors. // factors.
FactorGraph marginal; marginal.reserve(variables.size()); FACTORGRAPH marginal; marginal.reserve(variables.size());
typename FactorGraph::bayesnet_type::const_reverse_iterator conditional = bn->rbegin(); typename FACTORGRAPH::bayesnet_type::const_reverse_iterator conditional = bn->rbegin();
for(Index j=0; j<variables.size(); ++j, ++conditional) { 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); factor->permuteWithInverse(*permutation);
marginal.push_back(factor); marginal.push_back(factor);
assert(std::find(variables.begin(), variables.end(), (*permutation)[(*conditional)->key()]) != variables.end()); 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> template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
Permutation::shared_ptr Inference::PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast) { Permutation::shared_ptr Inference::PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast) {
size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
// Convert to compressed column major format colamd wants it in (== MATLAB format!) // 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 */ 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; p[0] = 0;
int count = 0; int count = 0;
for(Index var = 0; var < variableIndex.size(); ++var) { 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(); 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()) if(lastFactorId != numeric_limits<size_t>::max())
assert(factor_pos.factorIndex > lastFactorId); assert(factor_pos.factorIndex > lastFactorId);
A[count++] = factor_pos.factorIndex; // copy sparse column 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 * Eliminate a factor graph in its natural ordering, i.e. eliminating
* variables in order starting from 0. * variables in order starting from 0.
*/ */
template<class FactorGraph> template<class FACTORGRAPH>
static typename FactorGraph::bayesnet_type::shared_ptr Eliminate(const FactorGraph& factorGraph); static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(const FACTORGRAPH& factorGraph);
/** /**
* Eliminate a factor graph in its natural ordering, i.e. eliminating * 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 * variables in order starting from 0. Uses an existing
* variable index instead of building one from scratch. * variable index instead of building one from scratch.
*/ */
template<class FactorGraph> template<class FACTORGRAPH>
static typename FactorGraph::bayesnet_type::shared_ptr Eliminate( static typename FACTORGRAPH::bayesnet_type::shared_ptr Eliminate(
FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex); FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex);
/** /**
* Partially eliminate a factor graph, up to but not including the given * Partially eliminate a factor graph, up to but not including the given
* variable. * variable.
*/ */
template<class FactorGraph> template<class FACTORGRAPH>
static typename FactorGraph::bayesnet_type::shared_ptr static typename FACTORGRAPH::bayesnet_type::shared_ptr
EliminateUntil(const FactorGraph& factorGraph, Index bound); EliminateUntil(const FACTORGRAPH& factorGraph, Index bound);
/** /**
* Partially eliminate a factor graph, up to but not including the given * Partially eliminate a factor graph, up to but not including the given
* variable. Use an existing variable index instead of building one from * variable. Use an existing variable index instead of building one from
* scratch. * scratch.
*/ */
template<class FactorGraph> template<class FACTORGRAPH>
static typename FactorGraph::bayesnet_type::shared_ptr static typename FACTORGRAPH::bayesnet_type::shared_ptr
EliminateUntil(FactorGraph& factorGraph, Index bound, typename FactorGraph::variableindex_type& variableIndex); EliminateUntil(FACTORGRAPH& factorGraph, Index bound, typename FACTORGRAPH::variableindex_type& variableIndex);
/** /**
* Eliminate a single variable, updating an existing factor graph and * Eliminate a single variable, updating an existing factor graph and
* variable index. * variable index.
*/ */
template<class FactorGraph> template<class FACTORGRAPH>
static typename FactorGraph::bayesnet_type::sharedConditional static typename FACTORGRAPH::bayesnet_type::sharedConditional
EliminateOne(FactorGraph& factorGraph, typename FactorGraph::variableindex_type& variableIndex, Index var); EliminateOne(FACTORGRAPH& factorGraph, typename FACTORGRAPH::variableindex_type& variableIndex, Index var);
/** /**
* Eliminate a single variable, updating an existing factor graph and * Eliminate a single variable, updating an existing factor graph and
@ -101,24 +101,24 @@ namespace gtsam {
* BayesTree which supports efficiently computing marginals for multiple * BayesTree which supports efficiently computing marginals for multiple
* variables. * variables.
*/ */
template<class FactorGraph, class VarContainer> template<class FACTORGRAPH, class VARCONTAINER>
static FactorGraph Marginal(const FactorGraph& factorGraph, const VarContainer& variables); static FACTORGRAPH Marginal(const FACTORGRAPH& factorGraph, const VARCONTAINER& variables);
/** /**
* Compute a permutation (variable ordering) using colamd * Compute a permutation (variable ordering) using colamd
*/ */
template<class VariableIndexType> template<class VARIABLEINDEXTYPE>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VariableIndexType& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector<Index>()); } static boost::shared_ptr<Permutation> PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex) { return PermutationCOLAMD(variableIndex, std::vector<Index>()); }
template<class VariableIndexType, typename ConstraintContainer> template<class VARIABLEINDEXTYPE, typename CONSTRAINTCONTAINER>
static boost::shared_ptr<Permutation> PermutationCOLAMD(const VariableIndexType& variableIndex, const ConstraintContainer& constrainLast); static boost::shared_ptr<Permutation> PermutationCOLAMD(const VARIABLEINDEXTYPE& variableIndex, const CONSTRAINTCONTAINER& constrainLast);
// /** // /**
// * Join several factors into one. This involves determining the set of // * Join several factors into one. This involves determining the set of
// * shared variables and the correct variable positions in the new joint // * shared variables and the correct variable positions in the new joint
// * factor. // * factor.
// */ // */
// template<class FactorGraph, typename InputIterator> // template<class FACTORGRAPH, typename InputIterator>
// static typename FactorGraph::shared_factor Combine(const FactorGraph& factorGraph, // static typename FACTORGRAPH::shared_factor Combine(const FACTORGRAPH& factorGraph,
// InputIterator indicesBegin, InputIterator indicesEnd); // InputIterator indicesBegin, InputIterator indicesEnd);