gtsam  4.0.0
gtsam
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>
27 
28 #include <boost/shared_ptr.hpp>
29 #include <functional>
30 
31 namespace gtsam {
32 
33  // Forward declarations
34  class Values;
35  class Ordering;
36  class GaussianFactorGraph;
37  class SymbolicFactorGraph;
38  template<typename T>
39  class Expression;
40  template<typename T>
41  class ExpressionFactor;
42 
47  struct GTSAM_EXPORT GraphvizFormatting {
48  enum Axis { X, Y, Z, NEGX, NEGY, NEGZ };
53  double scale;
57  bool binaryEdges;
58  std::map<size_t, Point2> factorPositions;
62  paperHorizontalAxis(Y), paperVerticalAxis(X),
63  figureWidthInches(5), figureHeightInches(5), scale(1),
64  mergeSimilarFactors(false), plotFactorPoints(true),
65  connectKeysToFactor(true), binaryEdges(true) {}
66  };
67 
68 
77  class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
78 
79  public:
80 
82  typedef NonlinearFactorGraph This;
83  typedef boost::shared_ptr<This> shared_ptr;
84 
87 
89  template<typename ITERATOR>
90  NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
91 
93  template<class CONTAINER>
94  explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
95 
97  template<class DERIVEDFACTOR>
99 
101  void print(const std::string& str = "NonlinearFactorGraph: ",
102  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
103 
105  void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
106  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
107 
109  bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
110 
112  void saveGraph(std::ostream& stm, const Values& values = Values(),
113  const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
114  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
115 
117  double error(const Values& values) const;
118 
120  double probPrime(const Values& values) const;
121 
125  boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
126 
130  Ordering orderingCOLAMD() const;
131 
140  Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
141 
143  boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
144 
146  typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
147 
156  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
157  const Values& values, boost::optional<Ordering&> ordering = boost::none,
158  const Dampen& dampen = nullptr) const;
159 
162  Values updateCholesky(const Values& values, boost::optional<Ordering&> ordering = boost::none,
163  const Dampen& dampen = nullptr) const;
164 
166  NonlinearFactorGraph clone() const;
167 
177  NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
178 
185  template<typename T>
186  void addExpressionFactor(const SharedNoiseModel& R, const T& z,
187  const Expression<T>& h) {
188  push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
189  }
190 
191  private:
192 
194  friend class boost::serialization::access;
195  template<class ARCHIVE>
196  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
197  ar & boost::serialization::make_nvp("NonlinearFactorGraph",
198  boost::serialization::base_object<Base>(*this));
199  }
200  };
201 
203 template<>
204 struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
205 };
206 
207 } //\ namespace gtsam
208 
Axis paperVerticalAxis
The world axis assigned to the vertical paper axis.
Definition: NonlinearFactorGraph.h:50
std::map< size_t, Point2 > factorPositions
(optional for each factor) Manually specify factor "dot" positions.
Definition: NonlinearFactorGraph.h:58
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:70
bool mergeSimilarFactors
Merge multiple factors that have the same connectivity.
Definition: NonlinearFactorGraph.h:54
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
double scale
Scale all positions to reduce / increase density.
Definition: NonlinearFactorGraph.h:53
Formatting options when saving in GraphViz format using NonlinearFactorGraph::saveGraph.
Definition: NonlinearFactorGraph.h:47
Definition: FastMap.h:37
Template to create a binary predicate.
Definition: Testable.h:110
bool binaryEdges
just use non-dotted edges for binary factors
Definition: NonlinearFactorGraph.h:57
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
2D Point
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:33
Axis paperHorizontalAxis
The world axis assigned to the horizontal paper axis.
Definition: NonlinearFactorGraph.h:49
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition: NonlinearFactorGraph.h:90
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
NonlinearFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition: NonlinearFactorGraph.h:94
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: BayesTree.h:32
Non-linear factor base classes.
bool connectKeysToFactor
Draw a line from each key within a factor to the dot of the factor.
Definition: NonlinearFactorGraph.h:56
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition: NonlinearFactorGraph.h:146
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:77
Expression class that supports automatic differentiation.
Definition: Expression.h:49
Factor that supports arbitrary expressions via AD.
Definition: Expression.h:38
double figureHeightInches
The figure height on paper in inches.
Definition: NonlinearFactorGraph.h:52
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:1072
double figureWidthInches
The figure width on paper in inches.
Definition: NonlinearFactorGraph.h:51
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Directly add ExpressionFactor that implements |h(x)-z|^2_R.
Definition: NonlinearFactorGraph.h:186
Factor Graph Base Class.
NonlinearFactorGraph()
Default constructor.
Definition: NonlinearFactorGraph.h:86
Definition: Ordering.h:34
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition: NonlinearFactorGraph.h:98
bool plotFactorPoints
Plots each factor as a dot between the variables.
Definition: NonlinearFactorGraph.h:55