diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index e762786f5..a02fe274e 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -136,57 +136,61 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* *********************************************************************** */ // separator marginal, uses separator marginal of parent recursively // P(C) = P(F|S) P(S) - /* ************************************************************************* */ - template + /* *********************************************************************** */ + template typename BayesTreeCliqueBase::FactorGraphType - BayesTreeCliqueBase::separatorMarginal(Eliminate function) const - { + BayesTreeCliqueBase::separatorMarginal( + Eliminate function) const { gttic(BayesTreeCliqueBase_separatorMarginal); // Check if the Separator marginal was already calculated - if (!cachedSeparatorMarginal_) - { + if (!cachedSeparatorMarginal_) { gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); + // 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 FactorGraphType 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) // initialize P(Cp) with the parent separator marginal 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_cachemiss); + // 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 - KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); - cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); + KeyVector indicesS(this->conditional()->beginParents(), + this->conditional()->endParents()); + auto separatorMarginal = + p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); + cachedSeparatorMarginal_.reset(*separatorMarginal); } } // return the shortcut P(S||B) - 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) - /* ************************************************************************* */ - template + /* *********************************************************************** */ + template typename BayesTreeCliqueBase::FactorGraphType - BayesTreeCliqueBase::marginal2(Eliminate function) const - { + BayesTreeCliqueBase::marginal2( + Eliminate function) const { gttic(BayesTreeCliqueBase_marginal2); // initialize with separator marginal P(S) FactorGraphType p_C = this->separatorMarginal(function);