gtsam 4.2.0
gtsam
GaussianFactorGraph.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
22#pragma once
23
26#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
31
32namespace gtsam {
33
34 // Forward declarations
35 class GaussianFactorGraph;
36 class GaussianFactor;
38 class GaussianBayesNet;
39 class GaussianEliminationTree;
40 class GaussianBayesTree;
41 class GaussianJunctionTree;
42
43 /* ************************************************************************* */
45 {
54 static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
55 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
56 return EliminatePreferCholesky(factors, keys); }
59 const FactorGraphType& graph,
60 boost::optional<const VariableIndex&> variableIndex) {
61 return Ordering::Colamd(*variableIndex);
62 }
63 };
64
65 /* ************************************************************************* */
72 class GTSAM_EXPORT GaussianFactorGraph :
73 public FactorGraph<GaussianFactor>,
74 public EliminateableFactorGraph<GaussianFactorGraph>
75 {
76 public:
77
81 typedef boost::shared_ptr<This> shared_ptr;
82
85
88
94 GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
95
96
98 template<typename ITERATOR>
99 GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
100
102 template<class CONTAINER>
103 explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}
104
106 template<class DERIVEDFACTOR>
108
111
115
116 bool equals(const This& fg, double tol = 1e-9) const;
117
119
121 friend bool operator==(const GaussianFactorGraph& lhs,
122 const GaussianFactorGraph& rhs) {
123 return lhs.isEqual(rhs);
124 }
125
127 void add(const GaussianFactor& factor) { push_back(factor.clone()); }
128
130 void add(const sharedFactor& factor) { push_back(factor); }
131
133 void add(const Vector& b) {
134 add(JacobianFactor(b)); }
135
137 void add(Key key1, const Matrix& A1,
138 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
139 add(JacobianFactor(key1,A1,b,model)); }
140
142 void add(Key key1, const Matrix& A1,
143 Key key2, const Matrix& A2,
144 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
145 add(JacobianFactor(key1,A1,key2,A2,b,model)); }
146
148 void add(Key key1, const Matrix& A1,
149 Key key2, const Matrix& A2,
150 Key key3, const Matrix& A3,
151 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
152 add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); }
153
155 template<class TERMS>
156 void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
157 add(JacobianFactor(terms,b,model)); }
158
163 typedef KeySet Keys;
164 Keys keys() const;
165
166 /* return a map of (Key, dimension) */
167 std::map<Key, size_t> getKeyDimMap() const;
168
170 double error(const VectorValues& x) const;
171
173 double probPrime(const VectorValues& c) const;
174
180 virtual GaussianFactorGraph clone() const;
181
186 virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
187
194 GaussianFactorGraph negate() const;
195
198
209 std::vector<std::tuple<int, int, double> > sparseJacobian(
210 const Ordering& ordering, size_t& nrows, size_t& ncols) const;
211
213 std::vector<std::tuple<int, int, double> > sparseJacobian() const;
214
221 Matrix sparseJacobian_() const;
222
230 Matrix augmentedJacobian(const Ordering& ordering) const;
231
239 Matrix augmentedJacobian() const;
240
248 std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
249
257 std::pair<Matrix,Vector> jacobian() const;
258
270 Matrix augmentedHessian(const Ordering& ordering) const;
271
283 Matrix augmentedHessian() const;
284
291 std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
292
299 std::pair<Matrix,Vector> hessian() const;
300
302 virtual VectorValues hessianDiagonal() const;
303
305 virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
306
312 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
313
319 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
320
324 VectorValues optimizeDensely() const;
325
335 VectorValues gradient(const VectorValues& x0) const;
336
344 virtual VectorValues gradientAtZero() const;
345
370 VectorValues optimizeGradientSearch() const;
371
373 VectorValues transposeMultiply(const Errors& e) const;
374
376 void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
377
379 Errors gaussianErrors(const VectorValues& x) const;
380
382 Errors operator*(const VectorValues& x) const;
383
385 void multiplyHessianAdd(double alpha, const VectorValues& x,
386 VectorValues& y) const;
387
389 void multiplyInPlace(const VectorValues& x, Errors& e) const;
390
392 void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
393
394 void printErrors(
395 const VectorValues& x,
396 const std::string& str = "GaussianFactorGraph: ",
397 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
398 const std::function<bool(const Factor* /*factor*/,
399 double /*whitenedError*/, size_t /*index*/)>&
400 printCondition =
401 [](const Factor*, double, size_t) { return true; }) const;
403
404 private:
406 friend class boost::serialization::access;
407 template<class ARCHIVE>
408 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
409 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
410 }
411
412 public:
413
414#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
416 VectorValues GTSAM_DEPRECATED
417 optimize(boost::none_t, const Eliminate& function =
418 EliminationTraitsType::DefaultEliminate) const {
419 return optimize(function);
420 }
421#endif
422
423 };
424
429 GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
430
431 /****** Linear Algebra Operations ******/
432
434 //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
435 //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
436
438template<>
439struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
440};
441
442} // \ namespace gtsam
Factor Graph Base Class.
Variable elimination algorithms for factor graphs.
Factor Graph Values.
Contains the HessianFactor class, a general quadratic factor.
A factor with a quadratic error function - a Gaussian.
vector of errors
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition: HessianFactor.cpp:548
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
bool hasConstraints(const GaussianFactorGraph &factors)
Evaluates whether linear factors have any constrained noise models.
Definition: GaussianFactorGraph.cpp:442
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:155
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:47
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Template to create a binary predicate.
Definition: Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:97
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition: FactorGraph.h:134
boost::shared_ptr< GaussianFactor > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:101
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition: EliminateableFactorGraph.h:36
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition: EliminateableFactorGraph.h:57
Definition: Factor.h:68
Definition: Ordering.h:34
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition: Ordering.h:95
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition: GaussianBayesNet.h:36
A Bayes tree representing a Gaussian density.
Definition: GaussianBayesTree.h:52
A GaussianConditional functions as the node in a Bayes network.
Definition: GaussianConditional.h:43
Definition: GaussianEliminationTree.h:29
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy)
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition: GaussianFactorGraph.h:58
GaussianBayesTree BayesTreeType
Type of Bayes tree.
Definition: GaussianFactorGraph.h:51
GaussianConditional ConditionalType
Type of conditionals from elimination.
Definition: GaussianFactorGraph.h:48
GaussianFactor FactorType
Type of factors in factor graph.
Definition: GaussianFactorGraph.h:46
GaussianEliminationTree EliminationTreeType
Type of elimination tree.
Definition: GaussianFactorGraph.h:50
GaussianFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition: GaussianFactorGraph.h:47
GaussianBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition: GaussianFactorGraph.h:49
GaussianJunctionTree JunctionTreeType
Type of Junction tree.
Definition: GaussianFactorGraph.h:52
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition: GaussianFactorGraph.h:55
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:75
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition: GaussianFactorGraph.h:80
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:81
void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add an n-ary factor.
Definition: GaussianFactorGraph.h:156
GaussianFactorGraph(std::initializer_list< sharedFactor > factors)
Construct from an initializer lists of GaussianFactor shared pointers.
Definition: GaussianFactorGraph.h:94
GaussianFactorGraph()
Default constructor.
Definition: GaussianFactorGraph.h:87
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition: GaussianFactorGraph.h:127
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a ternary factor.
Definition: GaussianFactorGraph.h:148
void add(const sharedFactor &factor)
Add a factor by pointer - stores pointer without copying the factor.
Definition: GaussianFactorGraph.h:130
friend bool operator==(const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
Check exact equality.
Definition: GaussianFactorGraph.h:121
GaussianFactorGraph This
Typedef to this class.
Definition: GaussianFactorGraph.h:78
KeySet Keys
Return the set of variables involved in the factors (computes a set union).
Definition: GaussianFactorGraph.h:163
void add(const Vector &b)
Add a null factor.
Definition: GaussianFactorGraph.h:133
void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a unary factor.
Definition: GaussianFactorGraph.h:137
virtual ~GaussianFactorGraph()
Virtual destructor.
Definition: GaussianFactorGraph.h:110
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a binary factor.
Definition: GaussianFactorGraph.h:142
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition: GaussianFactorGraph.h:99
FactorGraph< GaussianFactor > Base
Typedef to base factor graph type.
Definition: GaussianFactorGraph.h:79
GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition: GaussianFactorGraph.h:107
GaussianFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition: GaussianFactorGraph.h:103
A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
Definition: GaussianJunctionTree.h:39
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition: VectorValues.h:74
is the normalization constant.