gtsam  4.0.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
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
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
60  Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY,
62
64  void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
65
67  GaussianFactor::shared_ptr marginalFactor(Key variable) const;
68
71  Matrix marginalInformation(Key variable) const;
72
74  Matrix marginalCovariance(Key variable) const;
75
77  JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
78
80  JointMarginal jointMarginalInformation(const KeyVector& variables) const;
81
83  VectorValues optimize() const;
84 };
85
89 class GTSAM_EXPORT JointMarginal {
90
91 protected:
92  SymmetricBlockMatrix blockMatrix_;
93  KeyVector keys_;
94  FastMap<Key, size_t> indices_;
95
96 public:
99
113  Matrix operator()(Key iVariable, Key jVariable) const {
114  const auto indexI = indices_.at(iVariable);
115  const auto indexJ = indices_.at(jVariable);
116  return blockMatrix_.block(indexI, indexJ);
117  }
118
120  Matrix at(Key iVariable, Key jVariable) const {
121  return (*this)(iVariable, jVariable);
122  }
123
125  Matrix fullMatrix() const {
127  }
128
130  void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
131
132 protected:
133  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
134  blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
135
136  friend class Marginals;
137
138 };
139
140 } /* namespace gtsam */
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:70
Matrix at(Key iVariable, Key jVariable) const
Synonym for operator()
Definition: Marginals.h:120
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
Matrix operator()(Key iVariable, Key jVariable) const
Access a block, corresponding to a pair of variables, of the joint marginal.
Definition: Marginals.h:113
A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
Definition: Marginals.h:89
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition: EliminateableFactorGraph.h:56
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
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:33
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:161
Factor Graph Constsiting of non-linear factors.
A Bayes tree representing a Gaussian density.
Definition: GaussianBayesTree.h:49
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
Factorization
The linear factorization mode - either CHOLESKY (faster and suitable for most problems) or QR (slower...
Definition: Marginals.h:37
JointMarginal()
Default constructor only for Cython wrapper.
Definition: Marginals.h:98
Marginals()
Default constructor only for Cython wrapper.
Definition: Marginals.h:52
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:56
A non-templated config holding any types of Manifold-group elements.
Matrix fullMatrix() const
The full, dense covariance/information matrix of the joint marginal.
Definition: Marginals.h:125
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:77
Definition: SymmetricBlockMatrix.h:51
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Matrix block(DenseIndex I, DenseIndex J) const
Get a copy of a block (anywhere in the matrix).
Definition: SymmetricBlockMatrix.cpp:54
A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
Definition: Marginals.h:32
Definition: Ordering.h:34
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42