gtsam 4.2.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
21#include <gtsam/base/timing.h>
22
23namespace 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>
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>
112 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
113 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
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 += 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 boost::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>
146 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
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
158 FactorGraphType 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)
170 gttic(BayesTreeCliqueBase_separatorMarginal);
171 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
172
173 // now add the parent conditional
174 p_Cp += parent->conditional_; // P(Fp|Sp)
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_.reset(*separatorMarginal);
182 }
183 }
184
185 // return the shortcut P(S||B)
186 return *cachedSeparatorMarginal_; // return the cached version
188
189 /* *********************************************************************** */
190 // marginal2, uses separator marginal of parent
191 // P(C) = P(F|S) P(S)
192 /* *********************************************************************** */
193 template <class DERIVED, class FACTORGRAPH>
194 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
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 += boost::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_ = boost::none;
221 }
222
223 }
224
225}
Timing utilities.
Base class for cliques of a BayesTree.
Factor Graph Base Class.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
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
A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
Definition: DiscreteFactorGraph.h:86
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition: BayesTreeCliqueBase-inst.h:84
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(C) of the clique, using marginal caching
Definition: BayesTreeCliqueBase-inst.h:195
bool equals(const DERIVED &other, double tol=1e-9) const
check equality
Definition: BayesTreeCliqueBase-inst.h:35
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition: BayesTreeCliqueBase-inst.h:147
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
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition: BayesTreeCliqueBase-inst.h:207
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
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition: BayesTreeCliqueBase-inst.h:45
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition: BayesTreeCliqueBase-inst.h:76
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition: BayesTreeCliqueBase-inst.h:93
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition: BayesTreeCliqueBase-inst.h:27
Definition: Ordering.h:34