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 
21 #include <gtsam/base/timing.h>
22 
23 namespace gtsam {
24 
25  /* ************************************************************************* */
26  template<class DERIVED, class FACTORGRAPH>
28  const typename FactorGraphType::EliminationResult& eliminationResult)
29  {
30  conditional_ = eliminationResult.first;
31  }
32 
33  /* ************************************************************************* */
34  template<class DERIVED, class FACTORGRAPH>
36  const DERIVED& other, double tol) const
37  {
38  return (!conditional_ && !other.conditional())
39  || conditional_->equals(*other.conditional(), tol);
40  }
41 
42  /* ************************************************************************* */
43  template<class DERIVED, class FACTORGRAPH>
44  KeyVector
46  {
47  KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
48  KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
49  KeyVector S_setminus_B;
50  std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
51  indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
52  return S_setminus_B;
53  }
54 
55  /* ************************************************************************* */
56  template<class DERIVED, class FACTORGRAPH>
58  const derived_ptr& B, const FactorGraphType& p_Cp_B) const
59  {
60  gttic(shortcut_indices);
61  KeySet allKeys = p_Cp_B.keys();
62  KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
63  KeyVector S_setminus_B = separator_setminus_B(B);
64  KeyVector keep;
65  // keep = S\B intersect allKeys (S_setminus_B is already sorted)
66  std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
67  allKeys.begin(), allKeys.end(), back_inserter(keep));
68  // keep += B intersect allKeys
69  std::set_intersection(indicesB.begin(), indicesB.end(), //
70  allKeys.begin(), allKeys.end(), back_inserter(keep));
71  return keep;
72  }
73 
74  /* ************************************************************************* */
75  template<class DERIVED, class FACTORGRAPH>
77  const std::string& s, const KeyFormatter& keyFormatter) const
78  {
79  conditional_->print(s, keyFormatter);
80  }
81 
82  /* ************************************************************************* */
83  template<class DERIVED, class FACTORGRAPH>
85  size_t size = 1;
86  for(const derived_ptr& child: children)
87  size += child->treeSize();
88  return size;
89  }
90 
91  /* ************************************************************************* */
92  template<class DERIVED, class FACTORGRAPH>
94  {
95  std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
96  if (!cachedSeparatorMarginal_)
97  return 0;
98 
99  size_t subtree_count = 1;
100  for(const derived_ptr& child: children)
101  subtree_count += child->numCachedSeparatorMarginals();
102 
103  return subtree_count;
104  }
105 
106  /* ************************************************************************* */
107  // The shortcut density is a conditional P(S|R) of the separator of this
108  // clique on the root. We can compute it recursively from the parent shortcut
109  // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
110  /* ************************************************************************* */
111  template<class DERIVED, class FACTORGRAPH>
114  {
115  gttic(BayesTreeCliqueBase_shortcut);
116  // We only calculate the shortcut when this clique is not B
117  // and when the S\B is not empty
118  KeyVector S_setminus_B = separator_setminus_B(B);
119  if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
120  {
121  // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
122  derived_ptr parent(parent_.lock());
123  gttoc(BayesTreeCliqueBase_shortcut);
124  FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
125  gttic(BayesTreeCliqueBase_shortcut);
126  p_Cp_B.push_back(parent->conditional_); // P(Fp|Sp)
127 
128  // Determine the variables we want to keepSet, S union B
129  KeyVector keep = shortcut_indices(B, p_Cp_B);
130 
131  // Marginalize out everything except S union B
132  std::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
133  return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first;
134  }
135  else
136  {
137  return BayesNetType();
138  }
139  }
140 
141  /* *********************************************************************** */
142  // separator marginal, uses separator marginal of parent recursively
143  // P(C) = P(F|S) P(S)
144  /* *********************************************************************** */
145  template <class DERIVED, class FACTORGRAPH>
148  Eliminate function) const {
149  std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
150  gttic(BayesTreeCliqueBase_separatorMarginal);
151  // Check if the Separator marginal was already calculated
152  if (!cachedSeparatorMarginal_) {
153  gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
154 
155  // If this is the root, there is no separator
156  if (parent_.expired() /*(if we're the root)*/) {
157  // we are root, return empty
159  cachedSeparatorMarginal_ = empty;
160  } else {
161  // Flatten recursion in timing outline
162  gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
163  gttoc(BayesTreeCliqueBase_separatorMarginal);
164 
165  // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
166  // initialize P(Cp) with the parent separator marginal
167  derived_ptr parent(parent_.lock());
168  FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
169 
170  gttic(BayesTreeCliqueBase_separatorMarginal);
171  gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
172 
173  // now add the parent conditional
174  p_Cp.push_back(parent->conditional_); // P(Fp|Sp)
175 
176  // The variables we want to keepSet are exactly the ones in S
177  KeyVector indicesS(this->conditional()->beginParents(),
178  this->conditional()->endParents());
179  auto separatorMarginal =
180  p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
181  cachedSeparatorMarginal_ = *separatorMarginal;
182  }
183  }
184 
185  // return the shortcut P(S||B)
186  return *cachedSeparatorMarginal_; // return the cached version
187  }
188 
189  /* *********************************************************************** */
190  // marginal2, uses separator marginal of parent
191  // P(C) = P(F|S) P(S)
192  /* *********************************************************************** */
193  template <class DERIVED, class FACTORGRAPH>
196  Eliminate function) const {
197  gttic(BayesTreeCliqueBase_marginal2);
198  // initialize with separator marginal P(S)
199  FactorGraphType p_C = this->separatorMarginal(function);
200  // add the conditional P(F|S)
201  p_C.push_back(std::shared_ptr<FactorType>(this->conditional_));
202  return p_C;
203  }
204 
205  /* ************************************************************************* */
206  template<class DERIVED, class FACTORGRAPH>
208 
209  // When a shortcut is requested, all of the shortcuts between it and the
210  // root are also generated. So, if this clique's cached shortcut is set,
211  // recursively call over all child cliques. Otherwise, it is unnecessary.
212 
213  std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
214  if (cachedSeparatorMarginal_) {
215  for(derived_ptr& child: children) {
216  child->deleteCachedShortcuts();
217  }
218 
219  //Delete CachedShortcut for this clique
220  cachedSeparatorMarginal_ = {};
221  }
222 
223  }
224 
225 }
gttoc
#define gttoc(label)
Definition: timing.h:296
timing.h
Timing utilities.
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::BayesTreeCliqueBase::setEliminationResult
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Definition: BayesTreeCliqueBase-inst.h:27
B
Definition: test_numpy_dtypes.cpp:299
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesNet
std::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:230
gtsam::BayesTreeCliqueBase::separatorMarginal
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
Definition: BayesTreeCliqueBase-inst.h:147
gtsam::BayesTreeCliqueBase::shortcut
BayesNetType shortcut(const derived_ptr &root, Eliminate function=EliminationTraitsType::DefaultEliminate) const
Definition: BayesTreeCliqueBase-inst.h:113
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
gtsam::EliminateableFactorGraph::marginal
std::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:440
gtsam::FastSet< Key >
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::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
FactorGraph-inst.h
Factor Graph Base Class.
gtsam::BayesTreeCliqueBase< SymbolicBayesTreeClique, SymbolicFactorGraph >::derived_ptr
std::shared_ptr< DerivedType > derived_ptr
Definition: BayesTreeCliqueBase.h:57
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::EliminateableFactorGraph< SymbolicFactorGraph >::EliminationResult
std::pair< std::shared_ptr< ConditionalType >, std::shared_ptr< _FactorType > > EliminationResult
Definition: EliminateableFactorGraph.h:85
gtsam::BayesTreeCliqueBase< SymbolicBayesTreeClique, SymbolicFactorGraph >::Eliminate
FactorGraphType::Eliminate Eliminate
Definition: BayesTreeCliqueBase.h:66
gtsam::BayesTreeCliqueBase::marginal2
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
Definition: BayesTreeCliqueBase-inst.h:195
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::SymbolicFactorGraph
Definition: SymbolicFactorGraph.h:61
gtsam::FactorGraph::keys
KeySet keys() const
Definition: FactorGraph-inst.h:85
gtsam::BayesTreeCliqueBase::treeSize
size_t treeSize() const
Definition: BayesTreeCliqueBase-inst.h:84
gtsam::BayesTreeCliqueBase::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: BayesTreeCliqueBase-inst.h:76
gtsam::BayesTreeCliqueBase::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Definition: BayesTreeCliqueBase-inst.h:93
gtsam::BayesTreeCliqueBase::BayesNetType
EliminationTraitsType::BayesNetType BayesNetType
Definition: BayesTreeCliqueBase.h:62
gtsam::BayesTreeCliqueBase::equals
bool equals(const DERIVED &other, double tol=1e-9) const
Definition: BayesTreeCliqueBase-inst.h:35
gtsam::BayesTreeCliqueBase::deleteCachedShortcuts
void deleteCachedShortcuts()
Definition: BayesTreeCliqueBase-inst.h:207
empty
Definition: test_copy_move.cpp:19
gtsam
traits
Definition: SFMdata.h:40
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::BayesTreeCliqueBase::shortcut_indices
KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) const
Definition: BayesTreeCliqueBase-inst.h:57
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::BayesTreeCliqueBase::FactorGraphType
FACTORGRAPH FactorGraphType
Definition: BayesTreeCliqueBase.h:61
gtsam::SymbolicBayesNet
Definition: SymbolicBayesNet.h:32
gtsam::Ordering
Definition: inference/Ordering.h:33
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gttic
#define gttic(label)
Definition: timing.h:295


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:00