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 
24 #include <cstddef>
27 #include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
32 
33 namespace gtsam {
34 
35  // Forward declarations
36  class GaussianFactorGraph;
37  class GaussianFactor;
38  class GaussianConditional;
39  class GaussianBayesNet;
40  class GaussianEliminationTree;
41  class GaussianBayesTree;
42  class GaussianJunctionTree;
43 
44  /* ************************************************************************* */
46  {
54  static std::pair<std::shared_ptr<ConditionalType>, std::shared_ptr<FactorType> >
60  const FactorGraphType& graph,
61  std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
62  return Ordering::Colamd((*variableIndex).get());
63  }
64  };
65 
66  /* ************************************************************************* */
73  class GTSAM_EXPORT GaussianFactorGraph :
74  public FactorGraph<GaussianFactor>,
75  public EliminateableFactorGraph<GaussianFactorGraph>
76  {
77  public:
78 
82  typedef std::shared_ptr<This> shared_ptr;
83 
86 
89 
95  GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
96 
97 
99  template<typename ITERATOR>
100  GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
101 
103  template<class CONTAINER>
104  explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}
105 
107  template<class DERIVEDFACTOR>
109 
113 
114  bool equals(const This& fg, double tol = 1e-9) const;
115 
117 
119  friend bool operator==(const GaussianFactorGraph& lhs,
120  const GaussianFactorGraph& rhs) {
121  return lhs.isEqual(rhs);
122  }
123 
125  void add(const GaussianFactor& factor) { push_back(factor.clone()); }
126 
128  void add(const sharedFactor& factor) { push_back(factor); }
129 
131  void add(const Vector& b) {
132  add(JacobianFactor(b)); }
133 
135  void add(Key key1, const Matrix& A1,
136  const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
138 
140  void add(Key key1, const Matrix& A1,
141  Key key2, const Matrix& A2,
142  const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
144 
146  void add(Key key1, const Matrix& A1,
147  Key key2, const Matrix& A2,
148  Key key3, const Matrix& A3,
149  const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
151 
153  template<class TERMS>
154  void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
155  add(JacobianFactor(terms,b,model)); }
156 
161  typedef KeySet Keys;
162  Keys keys() const;
163 
164  /* return a map of (Key, dimension) */
165  std::map<Key, size_t> getKeyDimMap() const;
166 
168  double error(const VectorValues& x) const;
169 
171  double probPrime(const VectorValues& c) const;
172 
178  virtual GaussianFactorGraph clone() const;
179 
184  virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
185 
192  GaussianFactorGraph negate() const;
193 
196 
207  std::vector<std::tuple<int, int, double> > sparseJacobian(
208  const Ordering& ordering, size_t& nrows, size_t& ncols) const;
209 
211  std::vector<std::tuple<int, int, double> > sparseJacobian() const;
212 
219  Matrix sparseJacobian_() const;
220 
228  Matrix augmentedJacobian(const Ordering& ordering) const;
229 
237  Matrix augmentedJacobian() const;
238 
246  std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
247 
255  std::pair<Matrix,Vector> jacobian() const;
256 
268  Matrix augmentedHessian(const Ordering& ordering) const;
269 
281  Matrix augmentedHessian() const;
282 
289  std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
290 
297  std::pair<Matrix,Vector> hessian() const;
298 
300  virtual VectorValues hessianDiagonal() const;
301 
303  virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
304 
310  const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
311 
317  const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
318 
322  VectorValues optimizeDensely() const;
323 
333  VectorValues gradient(const VectorValues& x0) const;
334 
342  virtual VectorValues gradientAtZero() const;
343 
368  VectorValues optimizeGradientSearch() const;
369 
371  VectorValues transposeMultiply(const Errors& e) const;
372 
374  void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
375 
377  Errors gaussianErrors(const VectorValues& x) const;
378 
380  Errors operator*(const VectorValues& x) const;
381 
383  void multiplyHessianAdd(double alpha, const VectorValues& x,
384  VectorValues& y) const;
385 
387  void multiplyInPlace(const VectorValues& x, Errors& e) const;
388 
390  void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
391 
392  void printErrors(
393  const VectorValues& x,
394  const std::string& str = "GaussianFactorGraph: ",
395  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
396  const std::function<bool(const Factor* /*factor*/,
397  double /*whitenedError*/, size_t /*index*/)>&
398  printCondition =
399  [](const Factor*, double, size_t) { return true; }) const;
401 
402  private:
403 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
404 
405  friend class boost::serialization::access;
406  template<class ARCHIVE>
407  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
408  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
409  }
410 #endif
411  };
412 
417  GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
418 
419  /****** Linear Algebra Operations ******/
420 
422  //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
423  //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
424 
426 template<>
427 struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
428 };
429 
430 } // \ namespace gtsam
key1
const Symbol key1('v', 1)
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
A3
static const double A3[]
Definition: expn.h:8
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, std::optional< std::reference_wrapper< const VariableIndex >> variableIndex)
The default ordering generation function.
Definition: GaussianFactorGraph.h:59
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const CONTAINER &factors)
Definition: GaussianFactorGraph.h:104
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Definition: triangulation.cpp:177
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Definition: GaussianFactorGraph.h:140
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::GaussianFactor
Definition: GaussianFactor.h:38
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Definition: GaussianFactorGraph.h:135
gtsam::GaussianJunctionTree
Definition: GaussianJunctionTree.h:38
gtsam::FactorGraph::isEqual
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition: FactorGraph.h:95
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::GaussianFactorGraph::add
void add(const Vector &b)
Definition: GaussianFactorGraph.h:131
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::EliminationTraits< GaussianFactorGraph >::JunctionTreeType
GaussianJunctionTree JunctionTreeType
Definition: GaussianFactorGraph.h:53
gtsam::Factor
Definition: Factor.h:69
gtsam::EliminationTraits< GaussianFactorGraph >::FactorGraphType
GaussianFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition: GaussianFactorGraph.h:48
gtsam::FastSet< Key >
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::GaussianFactorGraph::This
GaussianFactorGraph This
Typedef to this class.
Definition: GaussianFactorGraph.h:79
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
gtsam::FactorGraph
Definition: BayesTree.h:34
gtsam::EliminatePreferCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:540
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::GaussianFactorGraph::add
void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Definition: GaussianFactorGraph.h:154
gtsam::GaussianFactor::clone
virtual GaussianFactor::shared_ptr clone() const =0
gtsam::EliminationTraits< GaussianFactorGraph >::BayesTreeType
GaussianBayesTree BayesTreeType
Type of Bayes tree.
Definition: GaussianFactorGraph.h:52
GaussianConditional
gtsam::GaussianFactorGraph::Base
FactorGraph< GaussianFactor > Base
Typedef to base factor graph type.
Definition: GaussianFactorGraph.h:80
key2
const Symbol key2('v', 2)
gtsam::VectorValues
Definition: VectorValues.h:74
Eigen::internal::negate
T negate(const T &x)
Definition: packetmath_test_shared.h:24
gtsam::GaussianFactorGraph::add
void add(const sharedFactor &factor)
Definition: GaussianFactorGraph.h:128
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Definition: inference/Ordering.h:93
gtsam::GaussianFactorGraph::Keys
KeySet Keys
Definition: GaussianFactorGraph.h:161
gtsam::KeyFormatter
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
add
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
gtsam::GaussianConditional
Definition: GaussianConditional.h:40
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
A2
static const double A2[]
Definition: expn.h:7
key3
const Symbol key3('v', 3)
x0
static Symbol x0('x', 0)
Errors.h
vector of errors
VectorValues.h
Factor Graph Values.
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph()
Definition: GaussianFactorGraph.h:88
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultEliminate
static std::pair< std::shared_ptr< ConditionalType >, std::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition: GaussianFactorGraph.h:56
gtsam::FastList< Vector >
gtsam::EliminationTraits
Definition: BayesTreeCliqueBase.h:33
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
ordering
static enum @1096 ordering
gtsam::equals
Definition: Testable.h:112
y
Scalar * y
Definition: level1_cplx_impl.h:124
str
Definition: pytypes.h:1524
JacobianFactor.h
gtsam::b
const G & b
Definition: Group.h:79
gtsam::EliminationTraits< GaussianFactorGraph >::BayesNetType
GaussianBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition: GaussianFactorGraph.h:50
gtsam
traits
Definition: chartTesting.h:28
gtsam::Testable
Definition: Testable.h:152
gtsam::GaussianBayesTree
Definition: GaussianBayesTree.h:49
GaussianFactor
Continuous multi-dimensional vectors for.
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
FactorGraph.h
Factor Graph Base Class.
gtsam::GaussianFactorGraph::operator==
friend bool operator==(const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
Check exact equality.
Definition: GaussianFactorGraph.h:119
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Definition: GaussianFactorGraph.h:108
gtsam::EliminationTraits< GaussianFactorGraph >::ConditionalType
GaussianConditional ConditionalType
Type of conditionals from elimination.
Definition: GaussianFactorGraph.h:49
A1
static const double A1[]
Definition: expn.h:6
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::GaussianEliminationTree
Definition: GaussianEliminationTree.h:27
gtsam::tol
const G double tol
Definition: Group.h:79
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
Base
Definition: test_virtual_functions.cpp:156
gtsam::hasConstraints
bool hasConstraints(const GaussianFactorGraph &factors)
Definition: GaussianFactorGraph.cpp:442
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Definition: GaussianFactorGraph.h:100
gtsam::EliminateableFactorGraph
Definition: EliminateableFactorGraph.h:55
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::FactorGraph< GaussianFactor >::sharedFactor
std::shared_ptr< GaussianFactor > sharedFactor
Shared pointer to a factor.
Definition: FactorGraph.h:62
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(std::initializer_list< sharedFactor > factors)
Definition: GaussianFactorGraph.h:95
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())
Definition: GaussianFactorGraph.h:146
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
gtsam::GaussianBayesNet
Definition: GaussianBayesNet.h:35
gtsam::EliminationTraits< GaussianFactorGraph >::EliminationTreeType
GaussianEliminationTree EliminationTreeType
Type of elimination tree.
Definition: GaussianFactorGraph.h:51
gtsam::GaussianFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition: GaussianFactorGraph.h:81
gtsam::EliminationTraits< GaussianFactorGraph >::FactorType
GaussianFactor FactorType
Type of factors in factor graph.
Definition: GaussianFactorGraph.h:47


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:02:23