Loading...
Searching...
No Matches
BayesTreeCliqueBase-inst.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/base/timing.h>
namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::setEliminationResult(
const typename FactorGraphType::EliminationResult& eliminationResult)
{
conditional_ = eliminationResult.first;
}
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
bool BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::equals(
const DERIVED& other, double tol) const
{
return (!conditional_ && !other.conditional())
|| conditional_->equals(*other.conditional(), tol);
}
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
KeyVector
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{
KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
KeyVector S_setminus_B;
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
return S_setminus_B;
}
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
KeyVector BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
const derived_ptr& B, const FactorGraphType& p_Cp_B) const
{
gttic(shortcut_indices);
KeySet allKeys = p_Cp_B.keys();
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
KeyVector S_setminus_B = separator_setminus_B(B);
KeyVector keep;
// keep = S\B intersect allKeys (S_setminus_B is already sorted)
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
allKeys.begin(), allKeys.end(), back_inserter(keep));
// keep += B intersect allKeys
std::set_intersection(indicesB.begin(), indicesB.end(), //
allKeys.begin(), allKeys.end(), back_inserter(keep));
return keep;
}
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::print(
const std::string& s, const KeyFormatter& keyFormatter) const
{
conditional_->print(s, keyFormatter);
}
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
size_t size = 1;
for(const derived_ptr& child: children)
size += child->treeSize();
return size;
}
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
{
std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
if (!cachedSeparatorMarginal_)
return 0;
size_t subtree_count = 1;
for(const derived_ptr& child: children)
subtree_count += child->numCachedSeparatorMarginals();
return subtree_count;
}
/* ************************************************************************* */
// 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 FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
{
gttic(BayesTreeCliqueBase_shortcut);
// We only calculate the shortcut when this clique is not B
// and when the S\B is not empty
KeyVector S_setminus_B = separator_setminus_B(B);
if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
{
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_shortcut);
FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
gttic(BayesTreeCliqueBase_shortcut);
p_Cp_B += parent->conditional_; // P(Fp|Sp)
// Determine the variables we want to keepSet, S union B
KeyVector keep = shortcut_indices(B, p_Cp_B);
// Marginalize out everything except S union B
boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first;
}
else
{
return BayesNetType();
}
}
/* *********************************************************************** */
// separator marginal, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S)
/* *********************************************************************** */
template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(
Eliminate function) const {
std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
gttic(BayesTreeCliqueBase_separatorMarginal);
// Check if the Separator marginal was already calculated
if (!cachedSeparatorMarginal_) {
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// If this is the root, there is no separator
if (parent_.expired() /*(if we're the root)*/) {
// we are root, return empty
FactorGraphType empty;
cachedSeparatorMarginal_ = empty;
} 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());
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)
// The variables we want to keepSet are exactly the ones in S
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
}
/* *********************************************************************** */
// marginal2, uses separator marginal of parent
// P(C) = P(F|S) P(S)
/* *********************************************************************** */
template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(
Eliminate function) const {
gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S)
FactorGraphType p_C = this->separatorMarginal(function);
// add the conditional P(F|S)
p_C += boost::shared_ptr<FactorType>(this->conditional_);
return p_C;
}
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::deleteCachedShortcuts() {
// When a shortcut is requested, all of the shortcuts between it and the
// root are also generated. So, if this clique's cached shortcut is set,
// recursively call over all child cliques. Otherwise, it is unnecessary.
std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
if (cachedSeparatorMarginal_) {
for(derived_ptr& child: children) {
child->deleteCachedShortcuts();
}
//Delete CachedShortcut for this clique
cachedSeparatorMarginal_ = boost::none;
}
}
}
timing.h
Timing utilities.
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
FactorGraph-inst.h
Factor Graph Base Class.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::BayesTreeCliqueBase::parent
derived_ptr parent() const
return a shared_ptr to the parent clique
Definition BayesTreeCliqueBase.h:159
gtsam::BayesTreeCliqueBase::treeSize
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition BayesTreeCliqueBase-inst.h:84
gtsam::BayesTreeCliqueBase::cachedSeparatorMarginalMutex_
std::mutex cachedSeparatorMarginalMutex_
This protects Cached seperator marginal P(S) from concurrent read/writes as many the functions which ...
Definition BayesTreeCliqueBase.h:110
gtsam::BayesTreeCliqueBase::marginal2
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(C) of the clique, using marginal caching
Definition BayesTreeCliqueBase-inst.h:195
gtsam::BayesTreeCliqueBase::equals
bool equals(const DERIVED &other, double tol=1e-9) const
check equality
Definition BayesTreeCliqueBase-inst.h:35
gtsam::BayesTreeCliqueBase::separatorMarginal
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition BayesTreeCliqueBase-inst.h:147
gtsam::BayesTreeCliqueBase::shortcut
BayesNetType shortcut(const derived_ptr &root, Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the conditional P(S|Root) on the separator given the root
Definition BayesTreeCliqueBase-inst.h:113
gtsam::BayesTreeCliqueBase::deleteCachedShortcuts
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition BayesTreeCliqueBase-inst.h:207
gtsam::BayesTreeCliqueBase::shortcut_indices
KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) const
Determine variable indices to keep in recursive separator shortcut calculation The factor graph p_Cp_...
Definition BayesTreeCliqueBase-inst.h:57
gtsam::BayesTreeCliqueBase::conditional
const sharedConditional & conditional() const
Access the conditional.
Definition BayesTreeCliqueBase.h:141
gtsam::BayesTreeCliqueBase::separator_setminus_B
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition BayesTreeCliqueBase-inst.h:45
gtsam::BayesTreeCliqueBase::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition BayesTreeCliqueBase-inst.h:76
gtsam::BayesTreeCliqueBase::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTreeCliqueBase-inst.h:93
gtsam::BayesTreeCliqueBase::cachedSeparatorMarginal_
boost::optional< FactorGraphType > cachedSeparatorMarginal_
This stores the Cached separator marginal P(S).
Definition BayesTreeCliqueBase.h:105
gtsam::BayesTreeCliqueBase::setEliminationResult
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition BayesTreeCliqueBase-inst.h:27
gtsam::Ordering
Definition Ordering.h:34