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

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

@ -136,57 +136,61 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* *********************************************************************** */
// separator marginal, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
/* *********************************************************************** */
template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
{
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::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<class DERIVED, class FACTORGRAPH>
/* *********************************************************************** */
template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
{
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(
Eliminate function) const {
gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S)
FactorGraphType p_C = this->separatorMarginal(function);