NonlinearFactorGraph.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 
20 // \callgraph
21 
22 #pragma once
23 
24 #include <gtsam/geometry/Point2.h>
29 
30 #include <memory>
31 #include <functional>
32 
33 namespace gtsam {
34 
35  // Forward declarations
36  class Values;
37  class Ordering;
38  class GaussianFactorGraph;
39  class SymbolicFactorGraph;
40  template<typename T>
41  class Expression;
42  template<typename T>
43  class ExpressionFactor;
44 
55  class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
56 
57  public:
58 
61  typedef std::shared_ptr<This> shared_ptr;
62 
65 
68 
70  template<typename ITERATOR>
71  NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
72 
74  template<class CONTAINER>
75  explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
76 
78  template<class DERIVEDFACTOR>
80 
84 
86  void print(
87  const std::string& str = "NonlinearFactorGraph: ",
88  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
89 
91  void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
92  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
93  const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
94  printCondition = [](const Factor *,double, size_t) {return true;}) const;
95 
97  bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
98 
102 
104  double error(const Values& values) const;
105 
107  double probPrime(const Values& values) const;
108 
112  std::shared_ptr<SymbolicFactorGraph> symbolic() const;
113 
117  Ordering orderingCOLAMD() const;
118 
127  Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
128 
130  std::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
131 
133  typedef std::function<void(const std::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
134 
142  std::shared_ptr<HessianFactor> linearizeToHessianFactor(
143  const Values& values, const Dampen& dampen = nullptr) const;
144 
153  std::shared_ptr<HessianFactor> linearizeToHessianFactor(
154  const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
155 
158  Values updateCholesky(const Values& values,
159  const Dampen& dampen = nullptr) const;
160 
163  Values updateCholesky(const Values& values, const Ordering& ordering,
164  const Dampen& dampen = nullptr) const;
165 
167  NonlinearFactorGraph clone() const;
168 
178  NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
179 
186  template<typename T>
187  void addExpressionFactor(const SharedNoiseModel& R, const T& z,
188  const Expression<T>& h) {
189  this->emplace_shared<ExpressionFactor<T>>(R, z, h);
190  }
191 
198  template<typename T>
199  void addPrior(Key key, const T& prior,
200  const SharedNoiseModel& model = nullptr) {
201  emplace_shared<PriorFactor<T>>(key, prior, model);
202  }
203 
214  template<typename T>
215  void addPrior(Key key, const T& prior, const Matrix& covariance) {
216  emplace_shared<PriorFactor<T>>(key, prior, covariance);
217  }
218 
222 
223  using FactorGraph::dot;
225 
227  void dot(std::ostream& os, const Values& values,
228  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
229  const GraphvizFormatting& writer = GraphvizFormatting()) const;
230 
232  std::string dot(
233  const Values& values,
234  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
235  const GraphvizFormatting& writer = GraphvizFormatting()) const;
236 
238  void saveGraph(
239  const std::string& filename, const Values& values,
240  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
241  const GraphvizFormatting& writer = GraphvizFormatting()) const;
243 
244  private:
245 
250  std::shared_ptr<HessianFactor> linearizeToHessianFactor(
251  const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
252 
253 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
254 
255  friend class boost::serialization::access;
256  template<class ARCHIVE>
257  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
258  ar & boost::serialization::make_nvp("NonlinearFactorGraph",
259  boost::serialization::base_object<Base>(*this));
260  }
261 #endif
262  };
263 
265 template<>
266 struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
267 };
268 
269 } //\ namespace gtsam
270 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
void addPrior(Key key, const T &prior, const Matrix &covariance)
std::string serialize(const T &input)
serializes to a string
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
noiseModel::Diagonal::shared_ptr model
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
leaf::MyValues values
const GaussianFactorGraph factors
NonlinearFactorGraph graph
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
static enum @1107 ordering
Graphviz formatter for NonlinearFactorGraph.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Definition: pytypes.h:1403
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
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
traits
Definition: chartTesting.h:28
const double h
Non-linear factor base classes.
ofstream os("timeSchurFactors.csv")
FactorGraph< NonlinearFactor > Base
Factor Graph Base Class.
std::shared_ptr< This > shared_ptr
NonlinearFactorGraph(const CONTAINER &factors)
static double error
Definition: testRot3.cpp:37
const G double tol
Definition: Group.h:86
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
2D Point
std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


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