gtsam  4.1.0
gtsam
Marginals.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  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
23 #include <gtsam/nonlinear/Values.h>
24 
25 namespace gtsam {
26 
27 class JointMarginal;
28 
32 class GTSAM_EXPORT Marginals {
33 
34 public:
35 
38  CHOLESKY,
39  QR
40  };
41 
42 protected:
43 
44  GaussianFactorGraph graph_;
45  Values values_;
46  Factorization factorization_;
47  GaussianBayesTree bayesTree_;
48 
49 public:
50 
53 
59  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
60 
67  Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering,
68  Factorization factorization = CHOLESKY);
69 
75  Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
76 
83  Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering,
84  Factorization factorization = CHOLESKY);
85 
92  Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY);
93 
100  Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering,
101  Factorization factorization = CHOLESKY);
102 
104  void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
105 
107  GaussianFactor::shared_ptr marginalFactor(Key variable) const;
108 
111  Matrix marginalInformation(Key variable) const;
112 
114  Matrix marginalCovariance(Key variable) const;
115 
117  JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
118 
120  JointMarginal jointMarginalInformation(const KeyVector& variables) const;
121 
123  VectorValues optimize() const;
124 
125 protected:
126 
128  void computeBayesTree();
129 
131  void computeBayesTree(const Ordering& ordering);
132 
133 public:
135  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
136  const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
137 
139  Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
140  const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
141 
143  Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
144  const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
145 
146 };
147 
151 class GTSAM_EXPORT JointMarginal {
152 
153 protected:
154  SymmetricBlockMatrix blockMatrix_;
155  KeyVector keys_;
156  FastMap<Key, size_t> indices_;
157 
158 public:
161 
175  Matrix operator()(Key iVariable, Key jVariable) const {
176  const auto indexI = indices_.at(iVariable);
177  const auto indexJ = indices_.at(jVariable);
178  return blockMatrix_.block(indexI, indexJ);
179  }
180 
182  Matrix at(Key iVariable, Key jVariable) const {
183  return (*this)(iVariable, jVariable);
184  }
185 
187  Matrix fullMatrix() const {
188  return blockMatrix_.selfadjointView();
189  }
190 
192  void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
193 
194 protected:
195  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
196  blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
197 
198  friend class Marginals;
199 
200 };
201 
202 } /* namespace gtsam */
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
gtsam::Ordering
Definition: Ordering.h:34
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
gtsam::SymmetricBlockMatrix::block
Matrix block(DenseIndex I, DenseIndex J) const
Get a copy of a block (anywhere in the matrix).
Definition: SymmetricBlockMatrix.cpp:54
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Marginals::Marginals
Marginals(const GaussianFactorGraph &graph, const VectorValues &solution, Factorization factorization, const Ordering &ordering)
Definition: Marginals.h:143
gtsam::Marginals
A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
Definition: Marginals.h:32
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::JointMarginal::JointMarginal
JointMarginal()
Default constructor only for wrappers.
Definition: Marginals.h:160
gtsam::JointMarginal::fullMatrix
Matrix fullMatrix() const
The full, dense covariance/information matrix of the joint marginal.
Definition: Marginals.h:187
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::JointMarginal::at
Matrix at(Key iVariable, Key jVariable) const
Synonym for operator()
Definition: Marginals.h:182
gtsam::Marginals::Factorization
Factorization
The linear factorization mode - either CHOLESKY (faster and suitable for most problems) or QR (slower...
Definition: Marginals.h:37
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::KeyFormatter
boost::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::Marginals::Marginals
Marginals(const GaussianFactorGraph &graph, const Values &solution, Factorization factorization, const Ordering &ordering)
Definition: Marginals.h:139
gtsam::JointMarginal
A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
Definition: Marginals.h:151
gtsam::Marginals::Marginals
Marginals(const NonlinearFactorGraph &graph, const Values &solution, Factorization factorization, const Ordering &ordering)
Definition: Marginals.h:135
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:161
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::FastMap< Key, size_t >
gtsam::Marginals::Marginals
Marginals()
Default constructor only for wrappers.
Definition: Marginals.h:52
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition: GaussianBayesTree.h:52
gtsam::JointMarginal::operator()
Matrix operator()(Key iVariable, Key jVariable) const
Access a block, corresponding to a pair of variables, of the joint marginal.
Definition: Marginals.h:175