gtsam  4.1.0
gtsam
BayesTreeCliqueBase-inst.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9  * -------------------------------------------------------------------------- */
10 
17 #pragma once
18 
20 #include <gtsam/base/timing.h>
21 
22 namespace gtsam {
23 
24  /* ************************************************************************* */
25  template<class DERIVED, class FACTORGRAPH>
27  const typename FactorGraphType::EliminationResult& eliminationResult)
28  {
29  conditional_ = eliminationResult.first;
30  }
31 
32  /* ************************************************************************* */
33  template<class DERIVED, class FACTORGRAPH>
35  const DERIVED& other, double tol) const
36  {
37  return (!conditional_ && !other.conditional())
38  || conditional_->equals(*other.conditional(), tol);
39  }
40 
41  /* ************************************************************************* */
42  template<class DERIVED, class FACTORGRAPH>
43  KeyVector
45  {
46  KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
47  KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
48  KeyVector S_setminus_B;
49  std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
50  indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
51  return S_setminus_B;
52  }
53 
54  /* ************************************************************************* */
55  template<class DERIVED, class FACTORGRAPH>
57  const derived_ptr& B, const FactorGraphType& p_Cp_B) const
58  {
59  gttic(shortcut_indices);
60  KeySet allKeys = p_Cp_B.keys();
61  KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
62  KeyVector S_setminus_B = separator_setminus_B(B);
63  KeyVector keep;
64  // keep = S\B intersect allKeys (S_setminus_B is already sorted)
65  std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
66  allKeys.begin(), allKeys.end(), back_inserter(keep));
67  // keep += B intersect allKeys
68  std::set_intersection(indicesB.begin(), indicesB.end(), //
69  allKeys.begin(), allKeys.end(), back_inserter(keep));
70  return keep;
71  }
72 
73  /* ************************************************************************* */
74  template<class DERIVED, class FACTORGRAPH>
76  const std::string& s, const KeyFormatter& keyFormatter) const
77  {
78  conditional_->print(s, keyFormatter);
79  }
80 
81  /* ************************************************************************* */
82  template<class DERIVED, class FACTORGRAPH>
84  size_t size = 1;
85  for(const derived_ptr& child: children)
86  size += child->treeSize();
87  return size;
88  }
89 
90  /* ************************************************************************* */
91  template<class DERIVED, class FACTORGRAPH>
93  {
94  std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
95  if (!cachedSeparatorMarginal_)
96  return 0;
97 
98  size_t subtree_count = 1;
99  for(const derived_ptr& child: children)
100  subtree_count += child->numCachedSeparatorMarginals();
101 
102  return subtree_count;
103  }
104 
105  /* ************************************************************************* */
106  // The shortcut density is a conditional P(S|R) of the separator of this
107  // clique on the root. We can compute it recursively from the parent shortcut
108  // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
109  /* ************************************************************************* */
110  template<class DERIVED, class FACTORGRAPH>
111  typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
112  BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
113  {
114  gttic(BayesTreeCliqueBase_shortcut);
115  // We only calculate the shortcut when this clique is not B
116  // and when the S\B is not empty
117  KeyVector S_setminus_B = separator_setminus_B(B);
118  if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
119  {
120  // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
121  derived_ptr parent(parent_.lock());
122  gttoc(BayesTreeCliqueBase_shortcut);
123  FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
124  gttic(BayesTreeCliqueBase_shortcut);
125  p_Cp_B += parent->conditional_; // P(Fp|Sp)
126 
127  // Determine the variables we want to keepSet, S union B
128  KeyVector keep = shortcut_indices(B, p_Cp_B);
129 
130  // Marginalize out everything except S union B
131  boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
132  return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first;
133  }
134  else
135  {
136  return BayesNetType();
137  }
138  }
139 
140  /* *********************************************************************** */
141  // separator marginal, uses separator marginal of parent recursively
142  // P(C) = P(F|S) P(S)
143  /* *********************************************************************** */
144  template <class DERIVED, class FACTORGRAPH>
145  typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
147  Eliminate function) const {
148  std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
149  gttic(BayesTreeCliqueBase_separatorMarginal);
150  // Check if the Separator marginal was already calculated
151  if (!cachedSeparatorMarginal_) {
152  gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
153 
154  // If this is the root, there is no separator
155  if (parent_.expired() /*(if we're the root)*/) {
156  // we are root, return empty
158  cachedSeparatorMarginal_ = empty;
159  } else {
160  // Flatten recursion in timing outline
161  gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
162  gttoc(BayesTreeCliqueBase_separatorMarginal);
163 
164  // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
165  // initialize P(Cp) with the parent separator marginal
166  derived_ptr parent(parent_.lock());
167  FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
168 
169  gttic(BayesTreeCliqueBase_separatorMarginal);
170  gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
171 
172  // now add the parent conditional
173  p_Cp += parent->conditional_; // P(Fp|Sp)
174 
175  // The variables we want to keepSet are exactly the ones in S
176  KeyVector indicesS(this->conditional()->beginParents(),
177  this->conditional()->endParents());
178  auto separatorMarginal =
179  p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
180  cachedSeparatorMarginal_.reset(*separatorMarginal);
181  }
182  }
183 
184  // return the shortcut P(S||B)
185  return *cachedSeparatorMarginal_; // return the cached version
186  }
187 
188  /* *********************************************************************** */
189  // marginal2, uses separator marginal of parent
190  // P(C) = P(F|S) P(S)
191  /* *********************************************************************** */
192  template <class DERIVED, class FACTORGRAPH>
193  typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
195  Eliminate function) const {
196  gttic(BayesTreeCliqueBase_marginal2);
197  // initialize with separator marginal P(S)
198  FactorGraphType p_C = this->separatorMarginal(function);
199  // add the conditional P(F|S)
200  p_C += boost::shared_ptr<FactorType>(this->conditional_);
201  return p_C;
202  }
203 
204  /* ************************************************************************* */
205  template<class DERIVED, class FACTORGRAPH>
207 
208  // When a shortcut is requested, all of the shortcuts between it and the
209  // root are also generated. So, if this clique's cached shortcut is set,
210  // recursively call over all child cliques. Otherwise, it is unnecessary.
211 
212  std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
213  if (cachedSeparatorMarginal_) {
214  for(derived_ptr& child: children) {
215  child->deleteCachedShortcuts();
216  }
217 
218  //Delete CachedShortcut for this clique
219  cachedSeparatorMarginal_ = boost::none;
220  }
221 
222  }
223 
224 }
gtsam::SymbolicBayesNet
Symbolic Bayes Net.
Definition: SymbolicBayesNet.h:30
timing.h
Timing utilities.
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:112
gtsam::Ordering
Definition: Ordering.h:34
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::BayesTreeCliqueBase::separatorMarginal
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition: BayesTreeCliqueBase-inst.h:146
gtsam::BayesTreeCliqueBase::equals
bool equals(const DERIVED &other, double tol=1e-9) const
check equality
Definition: BayesTreeCliqueBase-inst.h:34
gtsam::BayesTreeCliqueBase::treeSize
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition: BayesTreeCliqueBase-inst.h:83
gtsam::FastSet< Key >
gtsam::KeyFormatter
boost::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::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::BayesTreeCliqueBase::deleteCachedShortcuts
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition: BayesTreeCliqueBase-inst.h:206
gtsam::BayesTreeCliqueBase::separator_setminus_B
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition: BayesTreeCliqueBase-inst.h:44
gtsam::BayesTreeCliqueBase::setEliminationResult
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition: BayesTreeCliqueBase-inst.h:26
gtsam::BayesTreeCliqueBase::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition: BayesTreeCliqueBase-inst.h:92
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:194
gtsam::SymbolicFactorGraph
Symbolic Factor Graph.
Definition: SymbolicFactorGraph.h:58
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:56
gtsam::BayesTreeCliqueBase::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition: BayesTreeCliqueBase-inst.h:75