DiscreteFactorGraph.cpp
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  * -------------------------------------------------------------------------- */
11 
27 
28 using std::vector;
29 using std::string;
30 using std::map;
31 
32 namespace gtsam {
33 
34  // Instantiate base classes
35  template class FactorGraph<DiscreteFactor>;
36  template class EliminateableFactorGraph<DiscreteFactorGraph>;
37 
38  /* ************************************************************************* */
39  bool DiscreteFactorGraph::equals(const This& fg, double tol) const
40  {
41  return Base::equals(fg, tol);
42  }
43 
44  /* ************************************************************************* */
46  KeySet keys;
47  for (const sharedFactor& factor : *this) {
48  if (factor) keys.insert(factor->begin(), factor->end());
49  }
50  return keys;
51  }
52 
53  /* ************************************************************************* */
56  for (auto&& factor : *this) {
57  if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
58  DiscreteKeys factor_keys = p->discreteKeys();
59  result.insert(result.end(), factor_keys.begin(), factor_keys.end());
60  }
61  }
62 
63  return result;
64  }
65 
66  /* ************************************************************************* */
69  for(const sharedFactor& factor: *this)
70  if (factor) result = (*factor) * result;
71  return result;
72  }
73 
74  /* ************************************************************************* */
76  const DiscreteValues &values) const {
77  double product = 1.0;
78  for( const sharedFactor& factor: factors_ )
79  product *= (*factor)(values);
80  return product;
81  }
82 
83  /* ************************************************************************* */
84  void DiscreteFactorGraph::print(const string& s,
85  const KeyFormatter& formatter) const {
86  std::cout << s << std::endl;
87  std::cout << "size: " << size() << std::endl;
88  for (size_t i = 0; i < factors_.size(); i++) {
89  std::stringstream ss;
90  ss << "factor " << i << ": ";
91  if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter);
92  }
93  }
94 
95 // /* ************************************************************************* */
96 // void DiscreteFactorGraph::permuteWithInverse(
97 // const Permutation& inversePermutation) {
98 // for(const sharedFactor& factor: factors_) {
99 // if(factor)
100 // factor->permuteWithInverse(inversePermutation);
101 // }
102 // }
103 //
104 // /* ************************************************************************* */
105 // void DiscreteFactorGraph::reduceWithInverse(
106 // const internal::Reduction& inverseReduction) {
107 // for(const sharedFactor& factor: factors_) {
108 // if(factor)
109 // factor->reduceWithInverse(inverseReduction);
110 // }
111 // }
112 
113  /* ************************************************************************ */
114  // Alternate eliminate function for MPE
115  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
117  const Ordering& frontalKeys) {
118  // PRODUCT: multiply all factors
119  gttic(product);
121  for (auto&& factor : factors) product = (*factor) * product;
122  gttoc(product);
123 
124  // Max over all the potentials by pretending all keys are frontal:
125  auto normalization = product.max(product.size());
126 
127  // Normalize the product factor to prevent underflow.
128  product = product / (*normalization);
129 
130  // max out frontals, this is the factor on the separator
131  gttic(max);
132  DecisionTreeFactor::shared_ptr max = product.max(frontalKeys);
133  gttoc(max);
134 
135  // Ordering keys for the conditional so that frontalKeys are really in front
136  DiscreteKeys orderedKeys;
137  for (auto&& key : frontalKeys)
138  orderedKeys.emplace_back(key, product.cardinality(key));
139  for (auto&& key : max->keys())
140  orderedKeys.emplace_back(key, product.cardinality(key));
141 
142  // Make lookup with product
143  gttic(lookup);
144  size_t nrFrontals = frontalKeys.size();
145  auto lookup = std::make_shared<DiscreteLookupTable>(nrFrontals,
146  orderedKeys, product);
147  gttoc(lookup);
148 
149  return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
150  }
151 
152  /* ************************************************************************ */
153  // sumProduct is just an alias for regular eliminateSequential.
155  OptionalOrderingType orderingType) const {
156  gttic(DiscreteFactorGraph_sumProduct);
157  auto bayesNet = eliminateSequential(orderingType);
158  return *bayesNet;
159  }
160 
162  const Ordering& ordering) const {
163  gttic(DiscreteFactorGraph_sumProduct);
165  return *bayesNet;
166  }
167 
168  /* ************************************************************************ */
169  // The max-product solution below is a bit clunky: the elimination machinery
170  // does not allow for differently *typed* versions of elimination, so we
171  // eliminate into a Bayes Net using the special eliminate function above, and
172  // then create the DiscreteLookupDAG after the fact, in linear time.
173 
175  OptionalOrderingType orderingType) const {
176  gttic(DiscreteFactorGraph_maxProduct);
177  auto bayesNet = eliminateSequential(orderingType, EliminateForMPE);
179  }
180 
182  const Ordering& ordering) const {
183  gttic(DiscreteFactorGraph_maxProduct);
186  }
187 
188  /* ************************************************************************ */
190  OptionalOrderingType orderingType) const {
191  gttic(DiscreteFactorGraph_optimize);
192  DiscreteLookupDAG dag = maxProduct(orderingType);
193  return dag.argmax();
194  }
195 
197  const Ordering& ordering) const {
198  gttic(DiscreteFactorGraph_optimize);
200  return dag.argmax();
201  }
202 
203  /* ************************************************************************ */
204  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
206  const Ordering& frontalKeys) {
207  // PRODUCT: multiply all factors
208  gttic(product);
210  for (auto&& factor : factors) product = (*factor) * product;
211  gttoc(product);
212 
213  // Max over all the potentials by pretending all keys are frontal:
214  auto normalization = product.max(product.size());
215 
216  // Normalize the product factor to prevent underflow.
217  product = product / (*normalization);
218 
219  // sum out frontals, this is the factor on the separator
220  gttic(sum);
221  DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
222  gttoc(sum);
223 
224  // Ordering keys for the conditional so that frontalKeys are really in front
225  Ordering orderedKeys;
226  orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(),
227  frontalKeys.end());
228  orderedKeys.insert(orderedKeys.end(), sum->keys().begin(),
229  sum->keys().end());
230 
231  // now divide product/sum to get conditional
232  gttic(divide);
233  auto conditional =
234  std::make_shared<DiscreteConditional>(product, *sum, orderedKeys);
235  gttoc(divide);
236 
237  return {conditional, sum};
238  }
239 
240  /* ************************************************************************ */
242  const KeyFormatter& keyFormatter,
243  const DiscreteFactor::Names& names) const {
244  using std::endl;
245  std::stringstream ss;
246  ss << "`DiscreteFactorGraph` of size " << size() << endl << endl;
247  for (size_t i = 0; i < factors_.size(); i++) {
248  ss << "factor " << i << ":\n";
249  ss << factors_[i]->markdown(keyFormatter, names) << endl;
250  }
251  return ss.str();
252  }
253 
254  /* ************************************************************************ */
255  string DiscreteFactorGraph::html(const KeyFormatter& keyFormatter,
256  const DiscreteFactor::Names& names) const {
257  using std::endl;
258  std::stringstream ss;
259  ss << "<div><p><tt>DiscreteFactorGraph</tt> of size " << size() << "</p>";
260  for (size_t i = 0; i < factors_.size(); i++) {
261  ss << "<p>factor " << i << ":</p>";
262  ss << factors_[i]->html(keyFormatter, names) << endl;
263  }
264  return ss.str();
265  }
266 
267  /* ************************************************************************ */
268  } // namespace gtsam
gtsam::DiscreteFactorGraph::discreteKeys
DiscreteKeys discreteKeys() const
Return the DiscreteKeys in this factor graph.
Definition: DiscreteFactorGraph.cpp:54
gttoc
#define gttoc(label)
Definition: timing.h:296
gtsam::EliminateableFactorGraph< DiscreteFactorGraph >::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
gtsam::DecisionTreeFactor
Definition: DecisionTreeFactor.h:44
gtsam::FactorGraph< DiscreteFactor >::factors_
FastVector< sharedFactor > factors_
Definition: FactorGraph.h:92
gtsam::DiscreteFactorGraph
Definition: DiscreteFactorGraph.h:98
gtsam::DiscreteFactorGraph::sumProduct
DiscreteBayesNet sumProduct(OptionalOrderingType orderingType={}) const
Implement the sum-product algorithm.
Definition: DiscreteFactorGraph.cpp:154
s
RealScalar s
Definition: level1_cplx_impl.h:126
DiscreteFactorGraph.h
gtsam::DiscreteFactorGraph::keys
KeySet keys() const
Definition: DiscreteFactorGraph.cpp:45
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
EliminateableFactorGraph-inst.h
DiscreteLookupDAG.h
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::DiscreteLookupDAG
Definition: DiscreteLookupDAG.h:77
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition: DiscreteKey.h:41
gtsam::DiscreteFactorGraph::print
void print(const std::string &s="DiscreteFactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: DiscreteFactorGraph.cpp:84
gtsam::FastSet< Key >
DiscreteConditional.h
gtsam::DecisionTreeFactor::shared_ptr
std::shared_ptr< DecisionTreeFactor > shared_ptr
Definition: DecisionTreeFactor.h:50
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::DiscreteFactorGraph::maxProduct
DiscreteLookupDAG maxProduct(OptionalOrderingType orderingType={}) const
Implement the max-product algorithm.
Definition: DiscreteFactorGraph.cpp:174
FactorGraph-inst.h
Factor Graph Base Class.
ss
static std::stringstream ss
Definition: testBTree.cpp:31
different_sigmas::bayesNet
const HybridBayesNet bayesNet
Definition: testHybridBayesNet.cpp:242
gtsam::DiscreteBayesNet
Definition: DiscreteBayesNet.h:38
gtsam::EliminateDiscrete
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for DiscreteFactorGraph.
Definition: DiscreteFactorGraph.cpp:205
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
DiscreteJunctionTree.h
gtsam::FactorGraph< DiscreteFactor >::equals
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
Definition: FactorGraph-inst.h:50
gtsam::DiscreteFactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Definition: DiscreteFactorGraph.cpp:39
gtsam::DiscreteFactorGraph::markdown
std::string markdown(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteFactor::Names &names={}) const
Render as markdown tables.
Definition: DiscreteFactorGraph.cpp:241
DiscreteEliminationTree.h
gtsam::EliminateForMPE
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateForMPE(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Alternate elimination function for that creates non-normalized lookup tables.
Definition: DiscreteFactorGraph.cpp:116
DiscreteBayesTree.h
Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
ordering
static enum @1096 ordering
key
const gtsam::Symbol key('X', 0)
gtsam::DiscreteFactorGraph::product
DecisionTreeFactor product() const
Definition: DiscreteFactorGraph.cpp:67
gtsam::FactorGraph< DiscreteFactor >::size
size_t size() const
Definition: FactorGraph.h:297
process_shonan_timing_results.names
dictionary names
Definition: process_shonan_timing_results.py:175
gtsam
traits
Definition: SFMdata.h:40
gtsam::DiscreteFactor::Names
DiscreteValues::Names Names
Translation table from values to strings.
Definition: DiscreteFactor.h:121
gtsam::DiscreteValues
Definition: DiscreteValues.h:34
gtsam::DiscreteValues::insert
std::pair< iterator, bool > insert(const value_type &value)
Definition: DiscreteValues.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::DiscreteFactorGraph::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteFactor::Names &names={}) const
Render as html tables.
Definition: DiscreteFactorGraph.cpp:255
product
void product(const MatrixType &m)
Definition: product.h:20
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::DiscreteLookupDAG::argmax
DiscreteValues argmax(DiscreteValues given=DiscreteValues()) const
argmax by back-substitution, optionally given certain variables.
Definition: DiscreteLookupDAG.cpp:120
gtsam::DiscreteFactorGraph::optimize
DiscreteValues optimize(OptionalOrderingType orderingType={}) const
Find the maximum probable explanation (MPE) by doing max-product.
Definition: DiscreteFactorGraph.cpp:189
gtsam::FactorGraph< DiscreteFactor >::sharedFactor
std::shared_ptr< DiscreteFactor > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:62
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::DiscreteLookupDAG::FromBayesNet
static DiscreteLookupDAG FromBayesNet(const DiscreteBayesNet &bayesNet)
Create from BayesNet with LookupTables.
Definition: DiscreteLookupDAG.cpp:105
gtsam::Ordering
Definition: inference/Ordering.h:33
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::DiscreteFactorGraph::operator()
double operator()(const DiscreteValues &values) const
Definition: DiscreteFactorGraph.cpp:75
gttic
#define gttic(label)
Definition: timing.h:295


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