Template arguements capitilized
							parent
							
								
									240889084b
								
							
						
					
					
						commit
						7d4f1ad268
					
				|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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) {} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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_); | ||||
| 	} | ||||
| }; | ||||
|  |  | |||
|  | @ -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())); } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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++)); | ||||
|   } | ||||
|  |  | |||
|  | @ -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); | ||||
| 	} | ||||
|  |  | |||
|  | @ -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
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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_; } | ||||
|  |  | |||
|  | @ -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_; | ||||
|  |  | |||
|  | @ -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) || | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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);
 | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue