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>
28 
29 #include <boost/shared_ptr.hpp>
30 #include <functional>
31 
32 namespace gtsam {
33 
34  // Forward declarations
35  class Values;
36  class Ordering;
37  class GaussianFactorGraph;
38  class SymbolicFactorGraph;
39  template<typename T>
40  class Expression;
41  template<typename T>
42  class ExpressionFactor;
43 
48  struct GTSAM_EXPORT GraphvizFormatting {
49  enum Axis { X, Y, Z, NEGX, NEGY, NEGZ };
54  double scale;
58  bool binaryEdges;
59  std::map<size_t, Point2> factorPositions;
63  paperHorizontalAxis(Y), paperVerticalAxis(X),
64  figureWidthInches(5), figureHeightInches(5), scale(1),
65  mergeSimilarFactors(false), plotFactorPoints(true),
66  connectKeysToFactor(true), binaryEdges(true) {}
67  };
68 
69 
78  class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
79 
80  public:
81 
84  typedef boost::shared_ptr<This> shared_ptr;
85 
88 
90  template<typename ITERATOR>
91  NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
92 
94  template<class CONTAINER>
95  explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
96 
98  template<class DERIVEDFACTOR>
100 
103 
105  void print(
106  const std::string& str = "NonlinearFactorGraph: ",
107  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
108 
110  void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
111  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
112  const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
113  printCondition = [](const Factor *,double, size_t) {return true;}) const;
114 
116  bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
117 
119  void saveGraph(std::ostream& stm, const Values& values = Values(),
120  const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
121  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
122 
129  void saveGraph(const std::string& file, const Values& values = Values(),
130  const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
131  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
132 
134  double error(const Values& values) const;
135 
137  double probPrime(const Values& values) const;
138 
142  boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
143 
147  Ordering orderingCOLAMD() const;
148 
157  Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
158 
160  boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
161 
163  typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
164 
172  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
173  const Values& values, const Dampen& dampen = nullptr) const;
174 
183  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
184  const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
185 
188  Values updateCholesky(const Values& values,
189  const Dampen& dampen = nullptr) const;
190 
193  Values updateCholesky(const Values& values, const Ordering& ordering,
194  const Dampen& dampen = nullptr) const;
195 
197  NonlinearFactorGraph clone() const;
198 
208  NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
209 
216  template<typename T>
217  void addExpressionFactor(const SharedNoiseModel& R, const T& z,
218  const Expression<T>& h) {
219  push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
220  }
221 
228  template<typename T>
229  void addPrior(Key key, const T& prior,
230  const SharedNoiseModel& model = nullptr) {
231  emplace_shared<PriorFactor<T>>(key, prior, model);
232  }
233 
244  template<typename T>
245  void addPrior(Key key, const T& prior, const Matrix& covariance) {
246  emplace_shared<PriorFactor<T>>(key, prior, covariance);
247  }
248 
249  private:
250 
255  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
256  const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
257 
259  friend class boost::serialization::access;
260  template<class ARCHIVE>
261  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
262  ar & boost::serialization::make_nvp("NonlinearFactorGraph",
263  boost::serialization::base_object<Base>(*this));
264  }
265 
266  public:
267 
269  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
270  const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
271  {return linearizeToHessianFactor(values, dampen);}
272 
274  Values updateCholesky(const Values& values, boost::none_t,
275  const Dampen& dampen = nullptr) const
276  {return updateCholesky(values, dampen);}
277 
278  };
279 
281 template<>
282 struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
283 };
284 
285 } //\ namespace gtsam
286 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&...args)
Definition: make_shared.h:57
bool mergeSimilarFactors
Merge multiple factors that have the same connectivity.
double scale
Scale all positions to reduce / increase density.
std::map< size_t, Point2 > factorPositions
void addPrior(Key key, const T &prior, const Matrix &covariance)
bool connectKeysToFactor
Draw a line from each key within a factor to the dot of the factor.
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:87
noiseModel::Diagonal::shared_ptr model
virtual ~NonlinearFactorGraph()
Destructor.
static enum @843 ordering
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Rot2 R(Rot2::fromAngle(0.1))
leaf::MyValues values
GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3))
NonlinearFactorGraph graph
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
bool plotFactorPoints
Plots each factor as a dot between the variables.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
boost::shared_ptr< This > shared_ptr
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Definition: pytypes.h:928
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
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Axis paperHorizontalAxis
The world axis assigned to the horizontal paper axis.
Axis paperVerticalAxis
The world axis assigned to the vertical paper axis.
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
bool binaryEdges
just use non-dotted edges for binary factors
traits
Definition: chartTesting.h:28
const double h
std::vector< float > Values
Non-linear factor base classes.
FactorGraph< NonlinearFactor > Base
Factor Graph Base Class.
NonlinearFactorGraph(const CONTAINER &factors)
double figureHeightInches
The figure height on paper in inches.
static double error
Definition: testRot3.cpp:39
Values updateCholesky(const Values &values, boost::none_t, const Dampen &dampen=nullptr) const
const G double tol
Definition: Group.h:83
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
#define X
Definition: icosphere.cpp:20
void serialize(ARCHIVE &ar, const unsigned int)
2D Point
boost::shared_ptr< HessianFactor > linearizeToHessianFactor(const Values &values, boost::none_t, const Dampen &dampen=nullptr) const
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
double figureWidthInches
The figure width on paper in inches.
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:03