Template Conditional->CONDITIONAL
							parent
							
								
									6ced775b4e
								
							
						
					
					
						commit
						01f73815e6
					
				|  | @ -32,7 +32,6 @@ | |||
| using namespace boost::assign; | ||||
| namespace lam = boost::lambda; | ||||
| 
 | ||||
| #include <gtsam/inference/Conditional.h> | ||||
| #include <gtsam/inference/BayesTree.h> | ||||
| #include <gtsam/inference/inference-inl.h> | ||||
| 
 | ||||
|  | @ -41,20 +40,20 @@ namespace gtsam { | |||
| 	using namespace std; | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	BayesTree<Conditional>::Clique::Clique() {} | ||||
| 	template<class CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::Clique::Clique() {} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	BayesTree<Conditional>::Clique::Clique(const sharedConditional& conditional) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::Clique::Clique(const sharedConditional& conditional) { | ||||
| 		separator_.assign(conditional->parents().begin(), conditional->parents().end()); | ||||
| 		this->push_back(conditional); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	BayesTree<Conditional>::Clique::Clique(const BayesNet<Conditional>& bayesNet) | ||||
| 	: BayesNet<Conditional>(bayesNet) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::Clique::Clique(const BayesNet<CONDITIONAL>& bayesNet) | ||||
| 	: BayesNet<CONDITIONAL>(bayesNet) { | ||||
| 	  if(bayesNet.size() > 0) { | ||||
| #ifndef NDEBUG | ||||
| 	    // Debug check that each parent variable is either a frontal variable in
 | ||||
|  | @ -63,13 +62,13 @@ namespace gtsam { | |||
| 	    // is "enough like a clique".  todo: this should really check that the
 | ||||
| 	    // fragment *is* a clique.
 | ||||
| 	    if(bayesNet.size() > 1) { | ||||
| 	      typename BayesNet<Conditional>::const_reverse_iterator cond = bayesNet.rbegin(); | ||||
| 	      typename BayesNet<CONDITIONAL>::const_reverse_iterator cond = bayesNet.rbegin(); | ||||
| 	      ++ cond; | ||||
| 	      typename Conditional::shared_ptr lastConditional = *cond; | ||||
| 	      typename CONDITIONAL::shared_ptr lastConditional = *cond; | ||||
| 	      for( ; cond != bayesNet.rend(); ++cond) | ||||
| 	        for(typename Conditional::const_iterator parent=(*cond)->beginParents(); parent!=(*cond)->endParents(); ++parent) { | ||||
| 	        for(typename CONDITIONAL::const_iterator parent=(*cond)->beginParents(); parent!=(*cond)->endParents(); ++parent) { | ||||
| 	          bool infragment = false; | ||||
| 	          typename BayesNet<Conditional>::const_reverse_iterator parentCond = cond; | ||||
| 	          typename BayesNet<CONDITIONAL>::const_reverse_iterator parentCond = cond; | ||||
| 	          do { | ||||
| 	            if(*parent == (*parentCond)->key()) | ||||
| 	              infragment = true; | ||||
|  | @ -86,8 +85,8 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	vector<Index> BayesTree<Conditional>::Clique::keys() const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	vector<Index> BayesTree<CONDITIONAL>::Clique::keys() const { | ||||
| 	  vector<Index> keys; | ||||
| 	  keys.reserve(this->size() + separator_.size()); | ||||
| 	  BOOST_FOREACH(const sharedConditional conditional, *this) { | ||||
|  | @ -98,8 +97,8 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::Clique::print(const string& s) const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::Clique::print(const string& s) const { | ||||
| 			cout << s << "Clique "; | ||||
| 			BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) { cout << conditional->key() << " "; } | ||||
| 			cout << "| "; | ||||
|  | @ -118,8 +117,8 @@ namespace gtsam { | |||
| 		} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	size_t BayesTree<Conditional>::Clique::treeSize() const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	size_t BayesTree<CONDITIONAL>::Clique::treeSize() const { | ||||
| 		size_t size = 1; | ||||
| 		BOOST_FOREACH(const shared_ptr& child, children_) | ||||
| 			size += child->treeSize(); | ||||
|  | @ -127,17 +126,17 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::Clique::printTree(const string& indent) const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::Clique::printTree(const string& indent) const { | ||||
| 		print(indent); | ||||
| 		BOOST_FOREACH(const shared_ptr& child, children_) | ||||
| 			child->printTree(indent+"  "); | ||||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   template<class Conditional> | ||||
|   void BayesTree<Conditional>::Clique::permuteWithInverse(const Permutation& inversePermutation) { | ||||
|     BayesNet<Conditional>::permuteWithInverse(inversePermutation); | ||||
|   template<class CONDITIONAL> | ||||
|   void BayesTree<CONDITIONAL>::Clique::permuteWithInverse(const Permutation& inversePermutation) { | ||||
|     BayesNet<CONDITIONAL>::permuteWithInverse(inversePermutation); | ||||
|     BOOST_FOREACH(Index& separatorKey, separator_) { separatorKey = inversePermutation[separatorKey]; } | ||||
|     if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); | ||||
|     BOOST_FOREACH(const shared_ptr& child, children_) { | ||||
|  | @ -146,9 +145,9 @@ namespace gtsam { | |||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   template<class Conditional> | ||||
|   bool BayesTree<Conditional>::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { | ||||
|     bool changed = BayesNet<Conditional>::permuteSeparatorWithInverse(inversePermutation); | ||||
|   template<class CONDITIONAL> | ||||
|   bool BayesTree<CONDITIONAL>::Clique::permuteSeparatorWithInverse(const Permutation& inversePermutation) { | ||||
|     bool changed = BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(inversePermutation); | ||||
| #ifndef NDEBUG | ||||
|     if(!changed) { | ||||
|       BOOST_FOREACH(Index& separatorKey, separator_) { assert(separatorKey == inversePermutation[separatorKey]); } | ||||
|  | @ -168,17 +167,17 @@ namespace gtsam { | |||
|   } | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	typename BayesTree<Conditional>::CliqueData | ||||
| 	BayesTree<Conditional>::getCliqueData() const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	typename BayesTree<CONDITIONAL>::CliqueData | ||||
| 	BayesTree<CONDITIONAL>::getCliqueData() const { | ||||
| 		CliqueData data; | ||||
| 		getCliqueData(data, root_); | ||||
| 		return data; | ||||
| 	} | ||||
| 
 | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::getCliqueData(CliqueData& data, | ||||
| 			BayesTree<Conditional>::sharedClique clique) const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::getCliqueData(CliqueData& data, | ||||
| 			BayesTree<CONDITIONAL>::sharedClique clique) const { | ||||
| 		data.conditionalSizes.push_back(clique->conditionals_.size()); | ||||
| 		data.separatorSizes.push_back(clique->separator_.size()); | ||||
| 		BOOST_FOREACH(sharedClique c, clique->children_) { | ||||
|  | @ -187,8 +186,8 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::saveGraph(const std::string &s) const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::saveGraph(const std::string &s) const { | ||||
| 		if (!root_.get()) throw invalid_argument("the root of bayes tree has not been initialized!"); | ||||
| 		ofstream of(s.c_str()); | ||||
| 		of<< "digraph G{\n"; | ||||
|  | @ -197,9 +196,9 @@ namespace gtsam { | |||
| 		of.close(); | ||||
| 	} | ||||
| 
 | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::saveGraph(ostream &s, | ||||
| 			BayesTree<Conditional>::sharedClique clique, | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::saveGraph(ostream &s, | ||||
| 			BayesTree<CONDITIONAL>::sharedClique clique, | ||||
| 			int parentnum) const { | ||||
| 		static int num = 0; | ||||
| 		bool first = true; | ||||
|  | @ -208,7 +207,7 @@ namespace gtsam { | |||
| 		string parent = out.str(); | ||||
| 		parent += "[label=\""; | ||||
| 
 | ||||
| 		BOOST_FOREACH(boost::shared_ptr<Conditional> c, clique->conditionals_) { | ||||
| 		BOOST_FOREACH(boost::shared_ptr<CONDITIONAL> c, clique->conditionals_) { | ||||
| 			if(!first) parent += ","; first = false; | ||||
| 			parent += (string(c->key())).c_str(); | ||||
| 		} | ||||
|  | @ -234,9 +233,9 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 
 | ||||
| 	template<class Conditional> | ||||
| 	typename BayesTree<Conditional>::CliqueStats | ||||
| 	BayesTree<Conditional>::CliqueData::getStats() const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	typename BayesTree<CONDITIONAL>::CliqueStats | ||||
| 	BayesTree<CONDITIONAL>::CliqueData::getStats() const { | ||||
| 		CliqueStats stats; | ||||
| 
 | ||||
| 		double sum = 0.0; | ||||
|  | @ -266,21 +265,21 @@ namespace gtsam { | |||
| 	// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
 | ||||
| 	// TODO, why do we actually return a shared pointer, why does eliminate?
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	template<class CONDITIONAL> | ||||
| 	template<class FactorGraph> | ||||
| 	BayesNet<Conditional> | ||||
| 	BayesTree<Conditional>::Clique::shortcut(shared_ptr R) { | ||||
| 	BayesNet<CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::Clique::shortcut(shared_ptr R) { | ||||
| 		// A first base case is when this clique or its parent is the root,
 | ||||
| 		// in which case we return an empty Bayes net.
 | ||||
| 
 | ||||
| 	  sharedClique parent(parent_.lock()); | ||||
| 
 | ||||
| 		if (R.get()==this || parent==R) { | ||||
| 			BayesNet<Conditional> empty; | ||||
| 			BayesNet<CONDITIONAL> empty; | ||||
| 			return empty; | ||||
| 		} | ||||
| 
 | ||||
| 		// 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
 | ||||
| 		FactorGraph p_Fp_Sp(*parent); | ||||
| 
 | ||||
|  | @ -327,7 +326,7 @@ namespace gtsam { | |||
| 		  factor->permuteWithInverse(*toFrontInverse); | ||||
| 		} | ||||
| 		varIndex.permute(toFront); | ||||
| 		BayesNet<Conditional> p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex); | ||||
| 		BayesNet<CONDITIONAL> p_S_R = *Inference::EliminateUntil(p_Cp_R, ordering.size(), varIndex); | ||||
| 
 | ||||
| 		// remove all integrands
 | ||||
| 		for(Index j=0; j<integrands.size(); ++j) | ||||
|  | @ -346,15 +345,15 @@ namespace gtsam { | |||
| 	// \int(Cp\S) P(F|S)P(S|Cp)P(Cp)
 | ||||
| 	// Because the root clique could be very big.
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	template<class CONDITIONAL> | ||||
| 	template<class 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 (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); | ||||
| 
 | ||||
|  | @ -365,14 +364,14 @@ namespace gtsam { | |||
| //	/* ************************************************************************* */
 | ||||
| //	// P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R)
 | ||||
| //	/* ************************************************************************* */
 | ||||
| //	template<class Conditional>
 | ||||
| //	template<class CONDITIONAL>
 | ||||
| //	template<class Factor>
 | ||||
| //	pair<FactorGraph<Factor>, Ordering>
 | ||||
| //	BayesTree<Conditional>::Clique::joint(shared_ptr C2, shared_ptr R) {
 | ||||
| //	BayesTree<CONDITIONAL>::Clique::joint(shared_ptr C2, shared_ptr R) {
 | ||||
| //		// For now, assume neither is the root
 | ||||
| //
 | ||||
| //		// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
 | ||||
| //		sharedBayesNet bn(new BayesNet<Conditional>);
 | ||||
| //		sharedBayesNet bn(new BayesNet<CONDITIONAL>);
 | ||||
| //		if (!isRoot())     bn->push_back(*this);                   // P(F1|S1)
 | ||||
| //		if (!isRoot())     bn->push_back(shortcut<Factor>(R));     // P(S1|R)
 | ||||
| //		if (!C2->isRoot()) bn->push_back(*C2);                     // P(F2|S2)
 | ||||
|  | @ -385,26 +384,26 @@ namespace gtsam { | |||
| //		keys12.unique();
 | ||||
| //
 | ||||
| //		// Calculate the marginal
 | ||||
| //		return make_pair(marginalize<Factor,Conditional>(*bn,keys12), keys12);
 | ||||
| //		return make_pair(marginalize<Factor,CONDITIONAL>(*bn,keys12), keys12);
 | ||||
| //	}
 | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::Cliques::print(const std::string& s) const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::Cliques::print(const std::string& s) const { | ||||
| 		cout << s << ":\n"; | ||||
| 		BOOST_FOREACH(sharedClique clique, *this) | ||||
| 				clique->printTree(); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	bool BayesTree<Conditional>::Cliques::equals(const Cliques& other, double tol) const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	bool BayesTree<CONDITIONAL>::Cliques::equals(const Cliques& other, double tol) const { | ||||
| 		return other == *this; | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::addClique( | ||||
| 	template<class CONDITIONAL> | ||||
| 	typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique( | ||||
| 	    const sharedConditional& conditional, sharedClique parent_clique) { | ||||
| 		sharedClique new_clique(new Clique(conditional)); | ||||
| 		Index key = conditional->key(); | ||||
|  | @ -418,8 +417,8 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::addClique( | ||||
| 	template<class CONDITIONAL> | ||||
| 	typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::addClique( | ||||
| 	    const sharedConditional& conditional, list<sharedClique>& child_cliques) { | ||||
| 		sharedClique new_clique(new Clique(conditional)); | ||||
|     Index key = conditional->key(); | ||||
|  | @ -432,8 +431,8 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	inline void BayesTree<Conditional>::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	inline void BayesTree<CONDITIONAL>::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) { | ||||
| 	  static const bool debug = false; | ||||
| #ifndef NDEBUG | ||||
| 	  // Debug check to make sure the conditional variable is ordered lower than
 | ||||
|  | @ -465,8 +464,8 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::removeClique(sharedClique clique) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::removeClique(sharedClique clique) { | ||||
| 
 | ||||
| 		if (clique->isRoot()) | ||||
| 			root_.reset(); | ||||
|  | @ -475,7 +474,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	  // orphan my children
 | ||||
| 		BOOST_FOREACH(sharedClique child, clique->children_) | ||||
| 	  	child->parent_ = typename BayesTree<Conditional>::Clique::weak_ptr(); | ||||
| 	  	child->parent_ = typename BayesTree<CONDITIONAL>::Clique::weak_ptr(); | ||||
| 
 | ||||
| 	  BOOST_FOREACH(Index key, clique->separator_) { | ||||
| 			nodes_[key].reset(); | ||||
|  | @ -487,34 +486,34 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	BayesTree<Conditional>::BayesTree() { | ||||
| 	template<class CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::BayesTree() { | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet) { | ||||
| 		typename BayesNet<Conditional>::const_reverse_iterator rit; | ||||
| 	template<class CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet) { | ||||
| 		typename BayesNet<CONDITIONAL>::const_reverse_iterator rit; | ||||
| 		for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) | ||||
| 			insert(*rit); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet, std::list<BayesTree<Conditional> > subtrees) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL> > subtrees) { | ||||
| 		if (bayesNet.size() == 0) | ||||
| 			throw invalid_argument("BayesTree::insert: empty bayes net!"); | ||||
| 
 | ||||
| 		// get the roots of child subtrees and merge their nodes_
 | ||||
| 		list<sharedClique> childRoots; | ||||
| 		BOOST_FOREACH(const BayesTree<Conditional>& subtree, subtrees) { | ||||
| 		BOOST_FOREACH(const BayesTree<CONDITIONAL>& subtree, subtrees) { | ||||
| 			nodes_.insert(subtree.nodes_.begin(), subtree.nodes_.end()); | ||||
| 			childRoots.push_back(subtree.root()); | ||||
| 		} | ||||
| 
 | ||||
| 		// create a new clique and add all the conditionals to the clique
 | ||||
| 		sharedClique new_clique; | ||||
| 		typename BayesNet<Conditional>::sharedConditional conditional; | ||||
| 		typename BayesNet<CONDITIONAL>::sharedConditional conditional; | ||||
| 		BOOST_REVERSE_FOREACH(conditional, bayesNet) { | ||||
| 			if (!new_clique.get()) | ||||
| 				new_clique = addClique(conditional,childRoots); | ||||
|  | @ -526,8 +525,8 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::print(const string& s) const { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::print(const string& s) const { | ||||
| 		if (root_.use_count() == 0) { | ||||
| 			printf("WARNING: BayesTree.print encountered a forest...\n"); | ||||
| 			return; | ||||
|  | @ -539,26 +538,26 @@ namespace gtsam { | |||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	// binary predicate to test equality of a pair for use in equals
 | ||||
| 	template<class Conditional> | ||||
| 	template<class CONDITIONAL> | ||||
| 	bool check_sharedCliques( | ||||
| 			const typename BayesTree<Conditional>::sharedClique& v1, | ||||
| 			const typename BayesTree<Conditional>::sharedClique& v2 | ||||
| 			const typename BayesTree<CONDITIONAL>::sharedClique& v1, | ||||
| 			const typename BayesTree<CONDITIONAL>::sharedClique& v2 | ||||
| 	) { | ||||
| 		return v1->equals(*v2); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	bool BayesTree<Conditional>::equals(const BayesTree<Conditional>& other, | ||||
| 	template<class CONDITIONAL> | ||||
| 	bool BayesTree<CONDITIONAL>::equals(const BayesTree<CONDITIONAL>& other, | ||||
| 			double tol) const { | ||||
| 		return size()==other.size() && | ||||
| 				std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<Conditional>); | ||||
| 				std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL>); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	template<class CONDITIONAL> | ||||
| 	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()); | ||||
| 	  assert(lowestOrderedParent != parents.end()); | ||||
| 	  return *lowestOrderedParent; | ||||
|  | @ -578,13 +577,13 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::insert(const sharedConditional& conditional) | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::insert(const sharedConditional& conditional) | ||||
| 	{ | ||||
| 	  static const bool debug = false; | ||||
| 
 | ||||
| 		// get key and parents
 | ||||
| 		typename Conditional::Parents parents = conditional->parents(); // todo: const reference?
 | ||||
| 		typename CONDITIONAL::Parents parents = conditional->parents(); // todo: const reference?
 | ||||
| 
 | ||||
| 		if(debug) conditional->print("Adding conditional "); | ||||
| 
 | ||||
|  | @ -627,9 +626,9 @@ namespace gtsam { | |||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	//TODO: remove this function after removing TSAM.cpp
 | ||||
| 	template<class Conditional> | ||||
| 	typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::insert( | ||||
| 			const BayesNet<Conditional>& bayesNet, list<sharedClique>& children, bool isRootClique) | ||||
| 	template<class CONDITIONAL> | ||||
| 	typename BayesTree<CONDITIONAL>::sharedClique BayesTree<CONDITIONAL>::insert( | ||||
| 			const BayesNet<CONDITIONAL>& bayesNet, list<sharedClique>& children, bool isRootClique) | ||||
| 	{ | ||||
| 	  static const bool debug = false; | ||||
| 
 | ||||
|  | @ -638,7 +637,7 @@ namespace gtsam { | |||
| 
 | ||||
| 		// create a new clique and add all the conditionals to the clique
 | ||||
| 		sharedClique new_clique; | ||||
| 		typename BayesNet<Conditional>::sharedConditional conditional; | ||||
| 		typename BayesNet<CONDITIONAL>::sharedConditional conditional; | ||||
| 		BOOST_REVERSE_FOREACH(conditional, bayesNet) { | ||||
| 		  if(debug) conditional->print("Inserting conditional: "); | ||||
| 			if (!new_clique.get()) | ||||
|  | @ -653,18 +652,18 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::fillNodesIndex(const sharedClique& subtree) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::fillNodesIndex(const sharedClique& subtree) { | ||||
| 	  // Add each frontal variable of this root node
 | ||||
| 	  BOOST_FOREACH(const typename Conditional::shared_ptr& cond, *subtree) { nodes_[cond->key()] = subtree; } | ||||
| 	  BOOST_FOREACH(const typename CONDITIONAL::shared_ptr& cond, *subtree) { nodes_[cond->key()] = subtree; } | ||||
| 	  // Fill index for each child
 | ||||
| 	  BOOST_FOREACH(const typename BayesTree<Conditional>::sharedClique& child, subtree->children_) { | ||||
| 	  BOOST_FOREACH(const typename BayesTree<CONDITIONAL>::sharedClique& child, subtree->children_) { | ||||
| 	    fillNodesIndex(child); } | ||||
| 	} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::insert(const sharedClique& subtree) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::insert(const sharedClique& subtree) { | ||||
| 	  if(subtree) { | ||||
| 	    // Find the parent clique of the new subtree.  By the running intersection
 | ||||
| 	    // property, those separator variables in the subtree that are ordered
 | ||||
|  | @ -690,10 +689,10 @@ namespace gtsam { | |||
| 	/* ************************************************************************* */ | ||||
| 	// First finds clique marginal then marginalizes that
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	template<class CONDITIONAL> | ||||
| 	template<class FactorGraph> | ||||
| 	FactorGraph | ||||
| 	BayesTree<Conditional>::marginal(Index key) const { | ||||
| 	BayesTree<CONDITIONAL>::marginal(Index key) const { | ||||
| 
 | ||||
| 		// get clique containing key
 | ||||
| 		sharedClique clique = (*this)[key]; | ||||
|  | @ -721,10 +720,10 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	template<class CONDITIONAL> | ||||
| 	template<class FactorGraph> | ||||
| 	BayesNet<Conditional> | ||||
| 	BayesTree<Conditional>::marginalBayesNet(Index key) const { | ||||
| 	BayesNet<CONDITIONAL> | ||||
| 	BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const { | ||||
| 
 | ||||
| 		// calculate marginal as a factor graph
 | ||||
| 	  FactorGraph fg = this->marginal<FactorGraph>(key); | ||||
|  | @ -736,10 +735,10 @@ namespace gtsam { | |||
| //	/* ************************************************************************* */
 | ||||
| //	// Find two cliques, their joint, then marginalizes
 | ||||
| //	/* ************************************************************************* */
 | ||||
| //	template<class Conditional>
 | ||||
| //	template<class CONDITIONAL>
 | ||||
| //	template<class Factor>
 | ||||
| //	FactorGraph<Factor>
 | ||||
| //	BayesTree<Conditional>::joint(Index key1, Index key2) const {
 | ||||
| //	BayesTree<CONDITIONAL>::joint(Index key1, Index key2) const {
 | ||||
| //
 | ||||
| //		// get clique C1 and C2
 | ||||
| //		sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
 | ||||
|  | @ -755,15 +754,15 @@ namespace gtsam { | |||
| //
 | ||||
| //		// partially eliminate, remaining factor graph is requested joint
 | ||||
| //		// TODO, make eliminate functional
 | ||||
| //		eliminate<Factor,Conditional>(p_C1C2,ord);
 | ||||
| //		eliminate<Factor,CONDITIONAL>(p_C1C2,ord);
 | ||||
| //		return p_C1C2;
 | ||||
| //	}
 | ||||
| 
 | ||||
| //	/* ************************************************************************* */
 | ||||
| //	template<class Conditional>
 | ||||
| //	template<class CONDITIONAL>
 | ||||
| //	template<class Factor>
 | ||||
| //	BayesNet<Conditional>
 | ||||
| //	BayesTree<Conditional>::jointBayesNet(Index key1, Index key2) const {
 | ||||
| //	BayesNet<CONDITIONAL>
 | ||||
| //	BayesTree<CONDITIONAL>::jointBayesNet(Index key1, Index key2) const {
 | ||||
| //
 | ||||
| //		// calculate marginal as a factor graph
 | ||||
| //	  FactorGraph<Factor> fg = this->joint<Factor>(key1,key2);
 | ||||
|  | @ -771,21 +770,21 @@ namespace gtsam { | |||
| //		// eliminate further to Bayes net
 | ||||
| //		Ordering ordering;
 | ||||
| //		ordering += key1, key2;
 | ||||
| //		return eliminate<Factor,Conditional>(fg,ordering);
 | ||||
| //		return eliminate<Factor,CONDITIONAL>(fg,ordering);
 | ||||
| //	}
 | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::clear() { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::clear() { | ||||
| 		// Remove all nodes and clear the root pointer
 | ||||
| 		nodes_.clear(); | ||||
| 		root_.reset(); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	void BayesTree<Conditional>::removePath(sharedClique clique, | ||||
| 			BayesNet<Conditional>& bn, typename BayesTree<Conditional>::Cliques& orphans) { | ||||
| 	template<class CONDITIONAL> | ||||
| 	void BayesTree<CONDITIONAL>::removePath(sharedClique clique, | ||||
| 			BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) { | ||||
| 
 | ||||
| 		// base case is NULL, if so we do nothing and return empties above
 | ||||
| 		if (clique!=NULL) { | ||||
|  | @ -808,10 +807,10 @@ namespace gtsam { | |||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class Conditional> | ||||
| 	template<class CONDITIONAL> | ||||
|   template<class Container> | ||||
|   void BayesTree<Conditional>::removeTop(const Container& keys, | ||||
|   		BayesNet<Conditional>& bn, typename BayesTree<Conditional>::Cliques& orphans) { | ||||
|   void BayesTree<CONDITIONAL>::removeTop(const Container& keys, | ||||
|   		BayesNet<CONDITIONAL>& bn, typename BayesTree<CONDITIONAL>::Cliques& orphans) { | ||||
| 
 | ||||
| 		// process each key of the new factor
 | ||||
| 	  BOOST_FOREACH(const Index& key, keys) { | ||||
|  |  | |||
|  | @ -35,38 +35,38 @@ namespace gtsam { | |||
| 
 | ||||
| 	/**
 | ||||
| 	 * Bayes tree | ||||
| 	 * Templated on the Conditional class, the type of node in the underlying Bayes chain. | ||||
| 	 * Templated on the CONDITIONAL class, the type of node in the underlying Bayes chain. | ||||
| 	 * This could be a ConditionalProbabilityTable, a GaussianConditional, or a SymbolicConditional | ||||
| 	 */ | ||||
| 	template<class Conditional> | ||||
| 	class BayesTree: public Testable<BayesTree<Conditional> > { | ||||
| 	template<class CONDITIONAL> | ||||
| 	class BayesTree: public Testable<BayesTree<CONDITIONAL> > { | ||||
| 
 | ||||
| 	public: | ||||
| 
 | ||||
| 		typedef boost::shared_ptr<Conditional> sharedConditional; | ||||
| 		typedef boost::shared_ptr<BayesNet<Conditional> > sharedBayesNet; | ||||
| 		typedef boost::shared_ptr<CONDITIONAL> sharedConditional; | ||||
| 		typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet; | ||||
| 
 | ||||
| 		/** A Clique in the tree is an incomplete Bayes net: the variables
 | ||||
| 		 * in the Bayes net are the frontal nodes, and the variables conditioned | ||||
| 		 * on is the separator. We also have pointers up and down the tree. | ||||
| 		 */ | ||||
| 		struct Clique: public BayesNet<Conditional> { | ||||
| 		struct Clique: public BayesNet<CONDITIONAL> { | ||||
| 
 | ||||
| 			typedef typename boost::shared_ptr<Clique> shared_ptr; | ||||
| 			typedef typename boost::weak_ptr<Clique> weak_ptr; | ||||
| 			weak_ptr parent_; | ||||
| 			std::list<shared_ptr> children_; | ||||
| 			std::list<Index> separator_; /** separator keys */ | ||||
| 			typename Conditional::Factor::shared_ptr cachedFactor_; | ||||
| 			typename CONDITIONAL::Factor::shared_ptr cachedFactor_; | ||||
| 
 | ||||
| 			friend class BayesTree<Conditional>; | ||||
| 			friend class BayesTree<CONDITIONAL>; | ||||
| 
 | ||||
| 			//* Constructor */
 | ||||
| 			Clique(); | ||||
| 
 | ||||
| 			Clique(const sharedConditional& conditional); | ||||
| 
 | ||||
| 			Clique(const BayesNet<Conditional>& bayesNet); | ||||
| 			Clique(const BayesNet<CONDITIONAL>& bayesNet); | ||||
| 
 | ||||
| 			/** return keys in frontal:separator order */ | ||||
| 			std::vector<Index> keys() const; | ||||
|  | @ -87,7 +87,7 @@ namespace gtsam { | |||
| 			const std::list<shared_ptr>& children() const { return children_; } | ||||
| 
 | ||||
| 			/** Reference the cached factor */ | ||||
| 			typename Conditional::Factor::shared_ptr& cachedFactor() { return cachedFactor_; } | ||||
| 			typename CONDITIONAL::Factor::shared_ptr& cachedFactor() { return cachedFactor_; } | ||||
| 
 | ||||
| 			/** The size of subtree rooted at this clique, i.e., nr of Cliques */ | ||||
| 			size_t treeSize() const; | ||||
|  | @ -108,7 +108,7 @@ namespace gtsam { | |||
| 			/** return the conditional P(S|Root) on the separator given the root */ | ||||
| 			// TODO: create a cached version
 | ||||
| 			template<class FactorGraph> | ||||
| 			BayesNet<Conditional> shortcut(shared_ptr root); | ||||
| 			BayesNet<CONDITIONAL> shortcut(shared_ptr root); | ||||
| 
 | ||||
| 			/** return the marginal P(C) of the clique */ | ||||
| 			template<class FactorGraph> | ||||
|  | @ -187,13 +187,13 @@ namespace gtsam { | |||
| 		BayesTree(); | ||||
| 
 | ||||
| 		/** Create a Bayes Tree from a Bayes Net */ | ||||
| 		BayesTree(const BayesNet<Conditional>& bayesNet); | ||||
| 		BayesTree(const BayesNet<CONDITIONAL>& bayesNet); | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Create a Bayes Tree from a Bayes Net and some subtrees. The Bayes net corresponds to the | ||||
| 		 * new root clique and the subtrees are connected to the root clique. | ||||
| 		 */ | ||||
| 		BayesTree(const BayesNet<Conditional>& bayesNet, std::list<BayesTree<Conditional> > subtrees); | ||||
| 		BayesTree(const BayesNet<CONDITIONAL>& bayesNet, std::list<BayesTree<CONDITIONAL> > subtrees); | ||||
| 
 | ||||
| 		/** Destructor */ | ||||
| 		virtual ~BayesTree() { | ||||
|  | @ -210,7 +210,7 @@ namespace gtsam { | |||
| 		 * It is the caller's responsibility to decide whether the given Bayes net is a valid clique, | ||||
| 		 * i.e. all the variables (frontal and separator) are connected | ||||
| 		 */ | ||||
| 		sharedClique insert(const BayesNet<Conditional>& bayesNet, | ||||
| 		sharedClique insert(const BayesNet<CONDITIONAL>& bayesNet, | ||||
| 				std::list<sharedClique>& children, bool isRootClique = false); | ||||
| 
 | ||||
| 		/**
 | ||||
|  | @ -227,7 +227,7 @@ namespace gtsam { | |||
| 		 */ | ||||
| 
 | ||||
| 		/** check equality */ | ||||
| 		bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const; | ||||
| 		bool equals(const BayesTree<CONDITIONAL>& other, double tol = 1e-9) const; | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Find parent clique of a conditional.  It will look at all parents and | ||||
|  | @ -262,7 +262,7 @@ namespace gtsam { | |||
| 
 | ||||
| 		/** return marginal on any variable, as a Bayes Net */ | ||||
| 		template<class FactorGraph> | ||||
| 		BayesNet<Conditional> marginalBayesNet(Index key) const; | ||||
| 		BayesNet<CONDITIONAL> marginalBayesNet(Index key) const; | ||||
| 
 | ||||
| //		/** return joint on two variables */
 | ||||
| //		template<class Factor>
 | ||||
|  | @ -270,7 +270,7 @@ namespace gtsam { | |||
| //
 | ||||
| //		/** return joint on two variables as a BayesNet */
 | ||||
| //		template<class Factor>
 | ||||
| //		BayesNet<Conditional> jointBayesNet(Index key1, Index key2) const;
 | ||||
| //		BayesNet<CONDITIONAL> jointBayesNet(Index key1, Index key2) const;
 | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Read only with side effects | ||||
|  | @ -293,14 +293,14 @@ namespace gtsam { | |||
| 		 * Remove path from clique to root and return that path as factors | ||||
| 		 * plus a list of orphaned subtree roots. Used in removeTop below. | ||||
| 		 */ | ||||
| 		void removePath(sharedClique clique, BayesNet<Conditional>& bn, Cliques& orphans); | ||||
| 		void removePath(sharedClique clique, BayesNet<CONDITIONAL>& bn, Cliques& orphans); | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * 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); | ||||
| 		void removeTop(const Container& keys, BayesNet<CONDITIONAL>& bn, Cliques& orphans); | ||||
| 
 | ||||
| 	}; // BayesTree
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue