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 
39  QR
40  };
41 
42 protected:
43 
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 
137 class GTSAM_EXPORT JointMarginal {
138 
139 protected:
143 
144 public:
147 
161  Matrix operator()(Key iVariable, Key jVariable) const {
162  const auto indexI = indices_.at(iVariable);
163  const auto indexJ = indices_.at(jVariable);
164  return blockMatrix_.block(indexI, indexJ);
165  }
166 
168  Matrix at(Key iVariable, Key jVariable) const {
169  return (*this)(iVariable, jVariable);
170  }
171 
173  Matrix fullMatrix() const {
174  return blockMatrix_.selfadjointView();
175  }
176 
178  void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
179 
180 protected:
181  JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
182  blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
183 
184  friend class Marginals;
185 
186 };
187 
188 } /* namespace gtsam */
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix at(Key iVariable, Key jVariable) const
Definition: Marginals.h:168
GaussianFactorGraph graph_
Definition: Marginals.h:44
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
NonlinearFactorGraph graph
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
static enum @1107 ordering
Values values_
Definition: Marginals.h:45
const KeyFormatter & formatter
Matrix fullMatrix() const
Definition: Marginals.h:173
JointMarginal(const Matrix &fullMatrix, const std::vector< size_t > &dims, const KeyVector &keys)
Definition: Marginals.h:181
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
GaussianBayesTree bayesTree_
Definition: Marginals.h:47
SymmetricBlockMatrix blockMatrix_
Definition: Marginals.h:140
FastMap< Key, size_t > indices_
Definition: Marginals.h:142
Definition: pytypes.h:1403
Marginals()
Default constructor only for wrappers.
Definition: Marginals.h:52
Matrix operator()(Key iVariable, Key jVariable) const
Definition: Marginals.h:161
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
RealScalar s
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()
Default constructor only for wrappers.
Definition: Marginals.h:146
std::shared_ptr< This > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
Factorization factorization_
Definition: Marginals.h:46
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Matrix block(DenseIndex I, DenseIndex J) const


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