gtsam  4.1.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 
29 #include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
30 
31 namespace gtsam {
32 
33  // Forward declarations
34  class GaussianFactorGraph;
35  class GaussianFactor;
36  class GaussianConditional;
37  class GaussianBayesNet;
38  class GaussianEliminationTree;
39  class GaussianBayesTree;
40  class GaussianJunctionTree;
41 
42  /* ************************************************************************* */
44  {
52  static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
54  DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
55  return EliminatePreferCholesky(factors, keys); }
56  };
57 
58  /* ************************************************************************* */
65  class GTSAM_EXPORT GaussianFactorGraph :
66  public FactorGraph<GaussianFactor>,
67  public EliminateableFactorGraph<GaussianFactorGraph>
68  {
69  public:
70 
74  typedef boost::shared_ptr<This> shared_ptr;
75 
78 
80  template<typename ITERATOR>
81  GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
82 
84  template<class CONTAINER>
85  explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}
86 
88  template<class DERIVEDFACTOR>
90 
92  virtual ~GaussianFactorGraph() {}
93 
96 
97  bool equals(const This& fg, double tol = 1e-9) const;
98 
100 
102  void add(const GaussianFactor& factor) { push_back(factor.clone()); }
103 
105  void add(const sharedFactor& factor) { push_back(factor); }
106 
108  void add(const Vector& b) {
109  add(JacobianFactor(b)); }
110 
112  void add(Key key1, const Matrix& A1,
113  const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
114  add(JacobianFactor(key1,A1,b,model)); }
115 
117  void add(Key key1, const Matrix& A1,
118  Key key2, const Matrix& A2,
119  const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
120  add(JacobianFactor(key1,A1,key2,A2,b,model)); }
121 
123  void add(Key key1, const Matrix& A1,
124  Key key2, const Matrix& A2,
125  Key key3, const Matrix& A3,
126  const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
127  add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); }
128 
130  template<class TERMS>
131  void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
132  add(JacobianFactor(terms,b,model)); }
133 
138  typedef KeySet Keys;
139  Keys keys() const;
140 
141  /* return a map of (Key, dimension) */
142  std::map<Key, size_t> getKeyDimMap() const;
143 
145  double error(const VectorValues& x) const {
146  double total_error = 0.;
147  for(const sharedFactor& factor: *this){
148  if(factor)
149  total_error += factor->error(x);
150  }
151  return total_error;
152  }
153 
155  double probPrime(const VectorValues& c) const {
156  return exp(-0.5 * error(c));
157  }
158 
164  virtual GaussianFactorGraph clone() const;
165 
170  virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
171 
178  GaussianFactorGraph negate() const;
179 
182 
188  std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;
189 
195  Matrix sparseJacobian_() const;
196 
204  Matrix augmentedJacobian(const Ordering& ordering) const;
205 
213  Matrix augmentedJacobian() const;
214 
222  std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
223 
231  std::pair<Matrix,Vector> jacobian() const;
232 
244  Matrix augmentedHessian(const Ordering& ordering) const;
245 
257  Matrix augmentedHessian() const;
258 
265  std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
266 
273  std::pair<Matrix,Vector> hessian() const;
274 
276  virtual VectorValues hessianDiagonal() const;
277 
279  virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
280 
286  const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
287 
293  const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
294 
298  VectorValues optimizeDensely() const;
299 
309  VectorValues gradient(const VectorValues& x0) const;
310 
318  virtual VectorValues gradientAtZero() const;
319 
344  VectorValues optimizeGradientSearch() const;
345 
347  VectorValues transposeMultiply(const Errors& e) const;
348 
350  void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
351 
353  Errors gaussianErrors(const VectorValues& x) const;
354 
356  Errors operator*(const VectorValues& x) const;
357 
359  void multiplyHessianAdd(double alpha, const VectorValues& x,
360  VectorValues& y) const;
361 
363  void multiplyInPlace(const VectorValues& x, Errors& e) const;
364 
366  void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
367 
369 
370  private:
372  friend class boost::serialization::access;
373  template<class ARCHIVE>
374  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
375  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
376  }
377 
378  public:
379 
381  VectorValues optimize(boost::none_t,
382  const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
383 
384  };
385 
390  GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
391 
392  /****** Linear Algebra Opeations ******/
393 
395  //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
396  //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
397 
399 template<>
400 struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
401 };
402 
403 } // \ namespace gtsam
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::EliminationTraits< GaussianFactorGraph >::EliminationTreeType
GaussianEliminationTree EliminationTreeType
Type of elimination tree.
Definition: GaussianFactorGraph.h:49
gtsam::GaussianFactorGraph::add
void add(const sharedFactor &factor)
Add a factor by pointer - stores pointer without copying the factor.
Definition: GaussianFactorGraph.h:105
JacobianFactor.h
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
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
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition: EliminateableFactorGraph.h:57
gtsam::EliminationTraits< GaussianFactorGraph >::FactorGraphType
GaussianFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition: GaussianFactorGraph.h:46
gtsam::hasConstraints
bool hasConstraints(const GaussianFactorGraph &factors)
Evaluates whether linear factors have any constrained noise models.
Definition: GaussianFactorGraph.cpp:416
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
gtsam::GaussianConditional
A conditional Gaussian functions as the node in a Bayes network It has a set of parents y,...
Definition: GaussianConditional.h:39
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::GaussianFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition: GaussianFactorGraph.h:73
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::GaussianFactorGraph::add
void add(const Vector &b)
Add a null factor.
Definition: GaussianFactorGraph.h:108
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
gtsam::GaussianFactorGraph::This
GaussianFactorGraph This
Typedef to this class.
Definition: GaussianFactorGraph.h:71
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
unnormalized error
Definition: GaussianFactorGraph.h:145
gtsam::GaussianJunctionTree
Definition: GaussianJunctionTree.h:37
gtsam::Errors
vector of errors
Definition: Errors.h:34
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a unary factor.
Definition: GaussianFactorGraph.h:112
gtsam::GaussianFactorGraph::~GaussianFactorGraph
virtual ~GaussianFactorGraph()
Virtual destructor.
Definition: GaussianFactorGraph.h:92
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition: GaussianFactorGraph.h:89
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph()
Default constructor.
Definition: GaussianFactorGraph.h:77
gtsam::GaussianFactorGraph::add
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:117
FactorGraph.h
Factor Graph Base Class.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::EliminationTraits< GaussianFactorGraph >::BayesNetType
GaussianBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition: GaussianFactorGraph.h:48
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition: GaussianFactorGraph.h:85
gtsam::GaussianFactorGraph::Base
FactorGraph< GaussianFactor > Base
Typedef to base factor graph type.
Definition: GaussianFactorGraph.h:72
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::GaussianBayesNet
A Bayes net made from linear-Gaussian densities.
Definition: GaussianBayesNet.h:31
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:94
gtsam::FactorGraph< GaussianFactor >::sharedFactor
boost::shared_ptr< GaussianFactor > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:98
gtsam::GaussianFactor::clone
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy)
gtsam::EliminationTraits< GaussianFactorGraph >::JunctionTreeType
GaussianJunctionTree JunctionTreeType
Type of Junction tree.
Definition: GaussianFactorGraph.h:51
gtsam::GaussianFactorGraph::probPrime
double probPrime(const VectorValues &c) const
Unnormalized probability.
Definition: GaussianFactorGraph.h:155
gtsam::FastSet< Key >
gtsam::GaussianFactorGraph::add
void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add an n-ary factor.
Definition: GaussianFactorGraph.h:131
gtsam::EliminationTraits< GaussianFactorGraph >::ConditionalType
GaussianConditional ConditionalType
Type of conditionals from elimination.
Definition: GaussianFactorGraph.h:47
gtsam::EliminationTraits< GaussianFactorGraph >::BayesTreeType
GaussianBayesTree BayesTreeType
Type of Bayes tree.
Definition: GaussianFactorGraph.h:50
gtsam::GaussianFactorGraph::add
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:123
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition: GaussianFactorGraph.h:81
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:45
gtsam::GaussianEliminationTree
Definition: GaussianEliminationTree.h:29
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:74
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultEliminate
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:54
gtsam::EliminationTraits< GaussianFactorGraph >::FactorType
GaussianFactor FactorType
Type of factors in factor graph.
Definition: GaussianFactorGraph.h:45
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition: EliminateableFactorGraph.h:36
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition: GaussianBayesTree.h:52
gtsam::GaussianFactorGraph::Keys
KeySet Keys
Return the set of variables involved in the factors (computes a set union).
Definition: GaussianFactorGraph.h:138
Errors.h
vector of errors
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition: GaussianFactorGraph.h:102