gtsam 4.1.1
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
22namespace 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>
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);
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
157 FactorGraphType empty;
158 cachedSeparatorMarginal_ = empty;
159 } else {
160 // Flatten recursion in timing outline
161 gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
162 gttoc(BayesTreeCliqueBase_separatorMarginal);
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 }
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}
Timing utilities.
Base class for cliques of a BayesTree.
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:66
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition: BayesTreeCliqueBase-inst.h:83
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(C) of the clique, using marginal caching
Definition: BayesTreeCliqueBase-inst.h:194
bool equals(const DERIVED &other, double tol=1e-9) const
check equality
Definition: BayesTreeCliqueBase-inst.h:34
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition: BayesTreeCliqueBase-inst.h:146
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
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition: BayesTreeCliqueBase-inst.h:206
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
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition: BayesTreeCliqueBase-inst.h:44
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition: BayesTreeCliqueBase-inst.h:75
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition: BayesTreeCliqueBase-inst.h:92
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition: BayesTreeCliqueBase-inst.h:26
Definition: Ordering.h:34