Minor change to shortcut
							parent
							
								
									2d0ce1c3ca
								
							
						
					
					
						commit
						bfc033d3f7
					
				|  | @ -102,114 +102,112 @@ namespace gtsam { | |||
|     return changed; | ||||
|   } | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	// The shortcut density is a conditional P(S|R) of the separator of this
 | ||||
| 	// clique on the root. We can compute it recursively from the parent shortcut
 | ||||
| 	// P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	template<class DERIVED, class CONDITIONAL> | ||||
| 	BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut( | ||||
| 			derived_ptr R, Eliminate function) const{ | ||||
|   /* ************************************************************************* */ | ||||
|   // The shortcut density is a conditional P(S|R) of the separator of this
 | ||||
|   // clique on the root. We can compute it recursively from the parent shortcut
 | ||||
|   // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
 | ||||
|   /* ************************************************************************* */ | ||||
|   template<class DERIVED, class CONDITIONAL> | ||||
|   BayesNet<CONDITIONAL> BayesTreeCliqueBase<DERIVED, CONDITIONAL>::shortcut( | ||||
|   		derived_ptr R, Eliminate function) const{ | ||||
| 
 | ||||
| 		static const bool debug = false; | ||||
|   	static const bool debug = false; | ||||
| 
 | ||||
| 		BayesNet<ConditionalType> p_S_R;	//shortcut P(S|R)
 | ||||
|   	BayesNet<ConditionalType> p_S_R;	//shortcut P(S|R) This is empty now
 | ||||
| 
 | ||||
| 		//Check if the ShortCut already exists
 | ||||
| 		if(!cachedShortcut_){ | ||||
|   	//Check if the ShortCut already exists
 | ||||
|   	if(!cachedShortcut_){ | ||||
| 
 | ||||
| 			// A first base case is when this clique or its parent is the root,
 | ||||
| 			// in which case we return an empty Bayes net.
 | ||||
|   		// A first base case is when this clique or its parent is the root,
 | ||||
|   		// in which case we return an empty Bayes net.
 | ||||
| 
 | ||||
| 			derived_ptr parent(parent_.lock()); | ||||
|   		derived_ptr parent(parent_.lock()); | ||||
|   		if (R.get() != this && parent != R) { | ||||
| 
 | ||||
| 			if (R.get() == this || parent == R) { | ||||
| 				BayesNet<ConditionalType> empty; | ||||
| 				return empty; | ||||
| 			} | ||||
|   			// The root conditional
 | ||||
|   			FactorGraph<FactorType> p_R(BayesNet<ConditionalType>(R->conditional())); | ||||
| 
 | ||||
| 			// The root conditional
 | ||||
| 			FactorGraph<FactorType> p_R(BayesNet<ConditionalType>(R->conditional())); | ||||
|   			// The parent clique has a ConditionalType for each frontal node in Fp
 | ||||
|   			// so we can obtain P(Fp|Sp) in factor graph form
 | ||||
|   			FactorGraph<FactorType> p_Fp_Sp(BayesNet<ConditionalType>(parent->conditional())); | ||||
| 
 | ||||
| 			// The parent clique has a ConditionalType for each frontal node in Fp
 | ||||
| 			// so we can obtain P(Fp|Sp) in factor graph form
 | ||||
| 			FactorGraph<FactorType> p_Fp_Sp(BayesNet<ConditionalType>(parent->conditional())); | ||||
|   			// If not the base case, obtain the parent shortcut P(Sp|R) as factors
 | ||||
|   			FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function)); | ||||
| 
 | ||||
| 			// If not the base case, obtain the parent shortcut P(Sp|R) as factors
 | ||||
| 			FactorGraph<FactorType> p_Sp_R(parent->shortcut(R, function)); | ||||
|   			// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
 | ||||
|   			FactorGraph<FactorType> p_Cp_R; | ||||
|   			p_Cp_R.push_back(p_R); | ||||
|   			p_Cp_R.push_back(p_Fp_Sp); | ||||
|   			p_Cp_R.push_back(p_Sp_R); | ||||
| 
 | ||||
| 			// now combine P(Cp|R) = P(Fp|Sp) * P(Sp|R)
 | ||||
| 			FactorGraph<FactorType> p_Cp_R; | ||||
| 			p_Cp_R.push_back(p_R); | ||||
| 			p_Cp_R.push_back(p_Fp_Sp); | ||||
| 			p_Cp_R.push_back(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
 | ||||
|   			// eliminated first hence the desired ordering is [Cp\S S].
 | ||||
|   			// However, an added wrinkle is that Cp might overlap with the root.
 | ||||
|   			// Keys corresponding to the root should not be added to the ordering at all.
 | ||||
| 
 | ||||
| 	    // Eliminate into a Bayes net with ordering designed to integrate out
 | ||||
| 	    // any variables not in *our* separator. Variables to integrate out must be
 | ||||
| 	    // eliminated first hence the desired ordering is [Cp\S S].
 | ||||
| 	    // However, an added wrinkle is that Cp might overlap with the root.
 | ||||
| 	    // Keys corresponding to the root should not be added to the ordering at all.
 | ||||
|   			if(debug) { | ||||
|   				p_R.print("p_R: "); | ||||
|   				p_Fp_Sp.print("p_Fp_Sp: "); | ||||
|   				p_Sp_R.print("p_Sp_R: "); | ||||
|   			} | ||||
| 
 | ||||
| 	    if(debug) { | ||||
| 	      p_R.print("p_R: "); | ||||
| 	      p_Fp_Sp.print("p_Fp_Sp: "); | ||||
| 	      p_Sp_R.print("p_Sp_R: "); | ||||
| 	    } | ||||
|   			// We want to factor into a conditional of the clique variables given the
 | ||||
|   			// root and the marginal on the root, integrating out all other variables.
 | ||||
|   			// The integrands include any parents of this clique and the variables of
 | ||||
|   			// the parent clique.
 | ||||
|   			FastSet<Index> variablesAtBack; | ||||
|   			FastSet<Index> separator; | ||||
|   			size_t uniqueRootVariables = 0; | ||||
|   			BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { | ||||
|   				variablesAtBack.insert(separatorIndex); | ||||
|   				separator.insert(separatorIndex); | ||||
|   				if(debug) std::cout << "At back (this): " << separatorIndex << std::endl; | ||||
|   			} | ||||
|   			BOOST_FOREACH(const Index key, R->conditional()->keys()) { | ||||
|   				if(variablesAtBack.insert(key).second) | ||||
|   					++ uniqueRootVariables; | ||||
|   				if(debug) std::cout << "At back (root): " << key << std::endl; | ||||
|   			} | ||||
| 
 | ||||
| 	    // We want to factor into a conditional of the clique variables given the
 | ||||
| 	    // root and the marginal on the root, integrating out all other variables.
 | ||||
| 	    // The integrands include any parents of this clique and the variables of
 | ||||
| 	    // the parent clique.
 | ||||
| 	    FastSet<Index> variablesAtBack; | ||||
| 	    FastSet<Index> separator; | ||||
| 	    size_t uniqueRootVariables = 0; | ||||
| 	    BOOST_FOREACH(const Index separatorIndex, this->conditional()->parents()) { | ||||
| 	      variablesAtBack.insert(separatorIndex); | ||||
| 	      separator.insert(separatorIndex); | ||||
| 	      if(debug) std::cout << "At back (this): " << separatorIndex << std::endl; | ||||
| 	    } | ||||
| 	    BOOST_FOREACH(const Index key, R->conditional()->keys()) { | ||||
| 	      if(variablesAtBack.insert(key).second) | ||||
| 	        ++ uniqueRootVariables; | ||||
| 	      if(debug) std::cout << "At back (root): " << key << std::endl; | ||||
| 	    } | ||||
|   			Permutation toBack = Permutation::PushToBack( | ||||
|   					std::vector<Index>(variablesAtBack.begin(), variablesAtBack.end()), | ||||
|   					R->conditional()->lastFrontalKey() + 1); | ||||
|   			Permutation::shared_ptr toBackInverse(toBack.inverse()); | ||||
|   			BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { | ||||
|   				factor->permuteWithInverse(*toBackInverse); } | ||||
|   			typename BayesNet<ConditionalType>::shared_ptr eliminated(EliminationTree< | ||||
|   					FactorType>::Create(p_Cp_R)->eliminate(function)); | ||||
| 
 | ||||
| 	    Permutation toBack = Permutation::PushToBack( | ||||
| 	        std::vector<Index>(variablesAtBack.begin(), variablesAtBack.end()), | ||||
| 	        R->conditional()->lastFrontalKey() + 1); | ||||
| 	    Permutation::shared_ptr toBackInverse(toBack.inverse()); | ||||
| 	    BOOST_FOREACH(const typename FactorType::shared_ptr& factor, p_Cp_R) { | ||||
| 	      factor->permuteWithInverse(*toBackInverse); } | ||||
| 	    typename BayesNet<ConditionalType>::shared_ptr eliminated(EliminationTree< | ||||
| 	        FactorType>::Create(p_Cp_R)->eliminate(function)); | ||||
|   			// Take only the conditionals for p(S|R).  We check for each variable being
 | ||||
|   			// in the separator set because if some separator variables overlap with
 | ||||
|   			// root variables, we cannot rely on the number of root variables, and also
 | ||||
|   			// want to include those variables in the conditional.
 | ||||
|   			BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) { | ||||
|   				assert(conditional->nrFrontals() == 1); | ||||
|   				if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { | ||||
|   					if(debug) | ||||
|   						conditional->print("Taking C|R conditional: "); | ||||
|   					p_S_R.push_front(conditional); | ||||
|   				} | ||||
|   				if(p_S_R.size() == separator.size()) | ||||
|   					break; | ||||
|   			} | ||||
| 
 | ||||
| 	    // Take only the conditionals for p(S|R).  We check for each variable being
 | ||||
| 	    // in the separator set because if some separator variables overlap with
 | ||||
| 	    // root variables, we cannot rely on the number of root variables, and also
 | ||||
| 	    // want to include those variables in the conditional.
 | ||||
| 	    BOOST_REVERSE_FOREACH(typename ConditionalType::shared_ptr conditional, *eliminated) { | ||||
| 	      assert(conditional->nrFrontals() == 1); | ||||
| 	      if(separator.find(toBack[conditional->firstFrontalKey()]) != separator.end()) { | ||||
| 	        if(debug) | ||||
| 	          conditional->print("Taking C|R conditional: "); | ||||
| 	        p_S_R.push_front(conditional); | ||||
| 	      } | ||||
| 	      if(p_S_R.size() == separator.size()) | ||||
| 	        break; | ||||
| 	    } | ||||
|   			// Undo the permutation
 | ||||
|   			if(debug) toBack.print("toBack: "); | ||||
|   			p_S_R.permuteWithInverse(toBack); | ||||
|   		} | ||||
| 
 | ||||
| 	    // Undo the permutation
 | ||||
| 	    if(debug) toBack.print("toBack: "); | ||||
| 	    p_S_R.permuteWithInverse(toBack); | ||||
|   		cachedShortcut_ = p_S_R; | ||||
|   	} | ||||
|   	else | ||||
|   		p_S_R = *cachedShortcut_;	// return the cached version
 | ||||
| 
 | ||||
| 	    assertInvariants(); | ||||
| 	    cachedShortcut_ = p_S_R; | ||||
| 		} | ||||
| 		else | ||||
| 			p_S_R = *cachedShortcut_; | ||||
|   	assertInvariants(); | ||||
| 
 | ||||
| 		// return the shortcut P(S|R)
 | ||||
|     return p_S_R; | ||||
|   	// return the shortcut P(S|R)
 | ||||
|   	return p_S_R; | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue