Marginals.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 
19 #include <gtsam/base/timing.h>
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 /* ************************************************************************* */
29 Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization)
30  : values_(solution), factorization_(factorization) {
31  gttic(MarginalsConstructor);
32  graph_ = *graph.linearize(solution);
34 }
35 
36 /* ************************************************************************* */
38  Factorization factorization)
39  : values_(solution), factorization_(factorization) {
40  gttic(MarginalsConstructor);
41  graph_ = *graph.linearize(solution);
42  computeBayesTree(ordering);
43 }
44 
45 /* ************************************************************************* */
46 Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization)
47  : graph_(graph), values_(solution), factorization_(factorization) {
48  gttic(MarginalsConstructor);
50 }
51 
52 /* ************************************************************************* */
54  Factorization factorization)
55  : graph_(graph), values_(solution), factorization_(factorization) {
56  gttic(MarginalsConstructor);
57  computeBayesTree(ordering);
58 }
59 
60 /* ************************************************************************* */
62  : graph_(graph), factorization_(factorization) {
63  gttic(MarginalsConstructor);
64  for (const auto& keyValue: solution) {
65  values_.insert(keyValue.first, keyValue.second);
66  }
68 }
69 
70 /* ************************************************************************* */
72  Factorization factorization)
73  : graph_(graph), factorization_(factorization) {
74  gttic(MarginalsConstructor);
75  for (const auto& keyValue: solution) {
76  values_.insert(keyValue.first, keyValue.second);
77  }
78  computeBayesTree(ordering);
79 }
80 
81 /* ************************************************************************* */
83  // The default ordering to use.
84  const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD;
85  // Compute BayesTree
86  if (factorization_ == CHOLESKY)
87  bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType,
89  else if (factorization_ == QR)
90  bayesTree_ =
91  *graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR);
92 }
93 
94 /* ************************************************************************* */
96  // Compute BayesTree
99  else if(factorization_ == QR)
101 }
102 
103 /* ************************************************************************* */
104 void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const
105 {
106  graph_.print(str+"Graph: ");
107  values_.print(str+"Solution: ", keyFormatter);
108  bayesTree_.print(str+"Bayes Tree: ");
109 }
110 
111 /* ************************************************************************* */
114 
115  // Compute marginal factor
116  if(factorization_ == CHOLESKY)
118  else if(factorization_ == QR)
119  return bayesTree_.marginalFactor(variable, EliminateQR);
120  else
121  throw std::runtime_error("Marginals::marginalFactor: Unknown factorization");
122 }
123 
124 /* ************************************************************************* */
126 
127  // Get information matrix (only store upper-right triangle)
129  return marginalFactor(variable)->information();
130 }
131 
132 /* ************************************************************************* */
134  return marginalInformation(variable).inverse();
135 }
136 
137 /* ************************************************************************* */
141  return info;
142 }
143 
144 /* ************************************************************************* */
146 
147  // If 2 variables, we can use the BayesTree::joint function, otherwise we
148  // have to use sequential elimination.
149  if(variables.size() == 1)
150  {
151  Matrix info = marginalInformation(variables.front());
152  std::vector<size_t> dims;
153  dims.push_back(info.rows());
154  return JointMarginal(info, dims, variables);
155  }
156  else
157  {
158  // Compute joint marginal factor graph.
159  GaussianFactorGraph jointFG;
160  if(variables.size() == 2) {
161  if(factorization_ == CHOLESKY)
162  jointFG = *bayesTree_.joint(variables[0], variables[1], EliminatePreferCholesky);
163  else if(factorization_ == QR)
164  jointFG = *bayesTree_.joint(variables[0], variables[1], EliminateQR);
165  } else {
166  if(factorization_ == CHOLESKY)
168  else if(factorization_ == QR)
170  }
171 
172  // Get information matrix
173  Matrix augmentedInfo = jointFG.augmentedHessian();
174  Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
175 
176  // Information matrix will be returned with sorted keys
177  KeyVector variablesSorted = variables;
178  std::sort(variablesSorted.begin(), variablesSorted.end());
179 
180  // Get dimensions from factor graph
181  std::vector<size_t> dims;
182  dims.reserve(variablesSorted.size());
183  for(const auto& key: variablesSorted) {
184  dims.push_back(values_.at(key).dim());
185  }
186 
187  return JointMarginal(info, dims, variablesSorted);
188  }
189 }
190 
191 /* ************************************************************************* */
193  return bayesTree_.optimize();
194 }
195 
196 /* ************************************************************************* */
197 void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const {
198  cout << s << "Joint marginal on keys ";
199  bool first = true;
200  for(const auto& key: keys_) {
201  if(!first)
202  cout << ", ";
203  else
204  first = false;
205  cout << formatter(key);
206  }
207  cout << ". Use 'at' or 'operator()' to query matrix blocks." << endl;
208 }
209 
210 } /* namespace gtsam */
const gtsam::Symbol key('X', 0)
void computeBayesTree()
Definition: Marginals.cpp:82
Matrix marginalCovariance(Key variable) const
Definition: Marginals.cpp:133
GaussianFactorGraph graph_
Definition: Marginals.h:44
const ValueType at(Key j) const
Definition: Values-inl.h:204
VectorValues optimize() const
Definition: Marginals.cpp:192
std::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Matrix marginalInformation(Key variable) const
Definition: Marginals.cpp:125
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: BFloat16.h:88
Matrix augmentedHessian(const Ordering &ordering) const
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const
Definition: Marginals.cpp:197
NonlinearFactorGraph graph
static enum @1107 ordering
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Values values_
Definition: Marginals.h:45
const KeyFormatter & formatter
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void invertInPlace()
Invert the entire active matrix in place.
else if n * info
JointMarginal jointMarginalCovariance(const KeyVector &variables) const
Definition: Marginals.cpp:138
#define gttic(label)
Definition: timing.h:295
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
GaussianBayesTree bayesTree_
Definition: Marginals.h:47
SymmetricBlockMatrix blockMatrix_
Definition: Marginals.h:140
VectorValues optimize() const
Definition: pytypes.h:1403
Marginals()
Default constructor only for wrappers.
Definition: Marginals.h:52
Contains the HessianFactor class, a general quadratic factor.
RealScalar s
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
OrderingType
Type of ordering to use.
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
JointMarginal jointMarginalInformation(const KeyVector &variables) const
Definition: Marginals.cpp:145
std::shared_ptr< This > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
Factorization factorization_
Definition: Marginals.h:46
std::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
void print(const std::string &str="Marginals: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Marginals.cpp:104
void insert(Key j, const Value &val)
Definition: Values.cpp:155
A class for computing marginals in a NonlinearFactorGraph.
sharedFactorGraph joint(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Timing utilities.
GaussianFactor::shared_ptr marginalFactor(Key variable) const
Definition: Marginals.cpp:112


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:45