gtsam  4.0.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(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
205 
213  std::pair<Matrix,Vector> jacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
214 
226  Matrix augmentedHessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
227 
234  std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
235 
237  virtual VectorValues hessianDiagonal() const;
238 
240  virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
241 
246  VectorValues optimize(OptionalOrdering ordering = boost::none,
247  const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
248 
252  VectorValues optimizeDensely() const;
253 
263  VectorValues gradient(const VectorValues& x0) const;
264 
272  virtual VectorValues gradientAtZero() const;
273 
298  VectorValues optimizeGradientSearch() const;
299 
301  VectorValues transposeMultiply(const Errors& e) const;
302 
304  void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
305 
307  Errors gaussianErrors(const VectorValues& x) const;
308 
310  Errors operator*(const VectorValues& x) const;
311 
313  void multiplyHessianAdd(double alpha, const VectorValues& x,
314  VectorValues& y) const;
315 
317  void multiplyInPlace(const VectorValues& x, Errors& e) const;
318 
320  void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
321 
323 
324  private:
326  friend class boost::serialization::access;
327  template<class ARCHIVE>
328  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
329  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
330  }
331 
332  };
333 
338  GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
339 
340  /****** Linear Algebra Opeations ******/
341 
343  //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
344  //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
345 
347 template<>
348 struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
349 };
350 
351 } // \ namespace gtsam
Definition: GaussianEliminationTree.h:27
void add(const Vector &b)
Add a null factor.
Definition: GaussianFactorGraph.h:108
A conditional Gaussian functions as the node in a Bayes network It has a set of parents y,...
Definition: GaussianConditional.h:36
FactorGraph< GaussianFactor > Base
Typedef to base factor graph type.
Definition: GaussianFactorGraph.h:72
vector of errors
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:74
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
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition: GaussianFactorGraph.h:81
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add an n-ary factor.
Definition: GaussianFactorGraph.h:131
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition: EliminateableFactorGraph.h:56
Template to create a binary predicate.
Definition: Testable.h:110
double probPrime(const VectorValues &c) const
Unnormalized probability.
Definition: GaussianFactorGraph.h:155
Definition: GaussianJunctionTree.h:36
GaussianConditional ConditionalType
Type of conditionals from elimination.
Definition: GaussianFactorGraph.h:47
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:38
KeySet Keys
Return the set of variables involved in the factors (computes a set union).
Definition: GaussianFactorGraph.h:138
vector of errors
Definition: Errors.h:34
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
GaussianEliminationTree EliminationTreeType
Type of elimination tree.
Definition: GaussianFactorGraph.h:49
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy)
void add(const sharedFactor &factor)
Add a factor by pointer - stores pointer without copying the factor.
Definition: GaussianFactorGraph.h:105
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition: GaussianFactorGraph.h:102
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
A Bayes net made from linear-Gaussian densities.
Definition: GaussianBayesNet.h:30
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: BayesTree.h:32
Contains the HessianFactor class, a general quadratic factor.
boost::shared_ptr< GaussianFactor > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:88
GaussianFactorGraph This
Typedef to this class.
Definition: GaussianFactorGraph.h:71
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition: GaussianFactorGraph.h:73
bool hasConstraints(const GaussianFactorGraph &factors)
Evaluates whether linear factors have any constrained noise models.
Definition: GaussianFactorGraph.cpp:383
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:87
A factor with a quadratic error function - a Gaussian.
Variable elimination algorithms for factor graphs.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
GaussianFactor FactorType
Type of factors in factor graph.
Definition: GaussianFactorGraph.h:45
void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a unary factor.
Definition: GaussianFactorGraph.h:112
virtual ~GaussianFactorGraph()
Virtual destructor.
Definition: GaussianFactorGraph.h:92
GaussianBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition: GaussianFactorGraph.h:48
Factor Graph Base Class.
GaussianFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition: GaussianFactorGraph.h:85
GaussianBayesTree BayesTreeType
Type of Bayes tree.
Definition: GaussianFactorGraph.h:50
Definition: Ordering.h:34
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition: BayesTreeCliqueBase.h:32
double error(const VectorValues &x) const
unnormalized error
Definition: GaussianFactorGraph.h:145
GaussianFactorGraph()
Default constructor.
Definition: GaussianFactorGraph.h:77
GaussianFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition: GaussianFactorGraph.h:46
Point2 operator *(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:170
GaussianJunctionTree JunctionTreeType
Type of Junction tree.
Definition: GaussianFactorGraph.h:51
GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition: GaussianFactorGraph.h:89