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

Cleaned up formatting

release/4.3a0
Frank dellaert 2020-07-12 15:00:54 -04:00
parent ae808d039c
commit d456dddc6f
1 changed files with 29 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -136,42 +136,46 @@ namespace gtsam {
} }
} }
/* ************************************************************************* */ /* *********************************************************************** */
// separator marginal, uses separator marginal of parent recursively // separator marginal, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* *********************************************************************** */
template<class DERIVED, class FACTORGRAPH> template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(
{ Eliminate function) const {
gttic(BayesTreeCliqueBase_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal);
// Check if the Separator marginal was already calculated // Check if the Separator marginal was already calculated
if (!cachedSeparatorMarginal_) if (!cachedSeparatorMarginal_) {
{
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// If this is the root, there is no separator // If this is the root, there is no separator
if (parent_.expired() /*(if we're the root)*/) if (parent_.expired() /*(if we're the root)*/) {
{
// we are root, return empty // we are root, return empty
FactorGraphType empty; FactorGraphType empty;
cachedSeparatorMarginal_ = empty; cachedSeparatorMarginal_ = empty;
} } else {
else // Flatten recursion in timing outline
{ gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
gttoc(BayesTreeCliqueBase_separatorMarginal);
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
// initialize P(Cp) with the parent separator marginal // initialize P(Cp) with the parent separator marginal
derived_ptr parent(parent_.lock()); derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBase_separatorMarginal);
FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp) FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
gttic(BayesTreeCliqueBase_separatorMarginal); gttic(BayesTreeCliqueBase_separatorMarginal);
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// now add the parent conditional // now add the parent conditional
p_Cp += parent->conditional_; // P(Fp|Sp) p_Cp += parent->conditional_; // P(Fp|Sp)
// The variables we want to keepSet are exactly the ones in S // The variables we want to keepSet are exactly the ones in S
KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); KeyVector indicesS(this->conditional()->beginParents(),
cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); this->conditional()->endParents());
auto separatorMarginal =
p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
cachedSeparatorMarginal_.reset(*separatorMarginal);
} }
} }
@ -179,14 +183,14 @@ namespace gtsam {
return *cachedSeparatorMarginal_; // return the cached version return *cachedSeparatorMarginal_; // return the cached version
} }
/* ************************************************************************* */ /* *********************************************************************** */
// marginal2, uses separator marginal of parent recursively // marginal2, uses separator marginal of parent
// P(C) = P(F|S) P(S) // P(C) = P(F|S) P(S)
/* ************************************************************************* */ /* *********************************************************************** */
template<class DERIVED, class FACTORGRAPH> template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(
{ Eliminate function) const {
gttic(BayesTreeCliqueBase_marginal2); gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S) // initialize with separator marginal P(S)
FactorGraphType p_C = this->separatorMarginal(function); FactorGraphType p_C = this->separatorMarginal(function);