gtsam  4.1.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>
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 
83  typedef NonlinearFactorGraph This;
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 
102  void print(const std::string& str = "NonlinearFactorGraph: ",
103  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
104 
106  void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
107  const KeyFormatter& keyFormatter = DefaultKeyFormatter,
108  const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
109  printCondition = [](const Factor *,double, size_t) {return true;}) const;
110 
112  bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
113 
115  void saveGraph(std::ostream& stm, const Values& values = Values(),
116  const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
117  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
118 
120  double error(const Values& values) const;
121 
123  double probPrime(const Values& values) const;
124 
128  boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
129 
133  Ordering orderingCOLAMD() const;
134 
143  Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
144 
146  boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
147 
149  typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
150 
158  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
159  const Values& values, const Dampen& dampen = nullptr) const;
160 
169  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
170  const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
171 
174  Values updateCholesky(const Values& values,
175  const Dampen& dampen = nullptr) const;
176 
179  Values updateCholesky(const Values& values, const Ordering& ordering,
180  const Dampen& dampen = nullptr) const;
181 
183  NonlinearFactorGraph clone() const;
184 
194  NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
195 
202  template<typename T>
203  void addExpressionFactor(const SharedNoiseModel& R, const T& z,
204  const Expression<T>& h) {
205  push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
206  }
207 
214  template<typename T>
215  void addPrior(Key key, const T& prior,
216  const SharedNoiseModel& model = nullptr) {
217  emplace_shared<PriorFactor<T>>(key, prior, model);
218  }
219 
230  template<typename T>
231  void addPrior(Key key, const T& prior, const Matrix& covariance) {
232  emplace_shared<PriorFactor<T>>(key, prior, covariance);
233  }
234 
235  private:
236 
241  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
242  const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
243 
245  friend class boost::serialization::access;
246  template<class ARCHIVE>
247  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
248  ar & boost::serialization::make_nvp("NonlinearFactorGraph",
249  boost::serialization::base_object<Base>(*this));
250  }
251 
252  public:
253 
255  boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
256  const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
257  {return linearizeToHessianFactor(values, dampen);}
258 
260  Values updateCholesky(const Values& values, boost::none_t,
261  const Dampen& dampen = nullptr) const
262  {return updateCholesky(values, dampen);}
263 
264  };
265 
267 template<>
268 struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
269 };
270 
271 } //\ namespace gtsam
272 
PriorFactor.h
gtsam::GraphvizFormatting::paperHorizontalAxis
Axis paperHorizontalAxis
The world axis assigned to the horizontal paper axis.
Definition: NonlinearFactorGraph.h:50
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::NonlinearFactorGraph::updateCholesky
Values updateCholesky(const Values &values, boost::none_t, const Dampen &dampen=nullptr) const
Definition: NonlinearFactorGraph.h:260
gtsam::Ordering
Definition: Ordering.h:34
gtsam::GraphvizFormatting::mergeSimilarFactors
bool mergeSimilarFactors
Merge multiple factors that have the same connectivity.
Definition: NonlinearFactorGraph.h:55
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph()
Default constructor.
Definition: NonlinearFactorGraph.h:87
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::GraphvizFormatting::connectKeysToFactor
bool connectKeysToFactor
Draw a line from each key within a factor to the dot of the factor.
Definition: NonlinearFactorGraph.h:57
FactorGraph.h
Factor Graph Base Class.
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition: NonlinearFactorGraph.h:95
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::GraphvizFormatting::factorPositions
std::map< size_t, Point2 > factorPositions
(optional for each factor) Manually specify factor "dot" positions.
Definition: NonlinearFactorGraph.h:59
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const Matrix &covariance)
Convenience method which adds a PriorFactor to the factor graph.
Definition: NonlinearFactorGraph.h:231
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Convenience method which adds a PriorFactor to the factor graph.
Definition: NonlinearFactorGraph.h:215
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition: NonlinearFactorGraph.h:99
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NonlinearFactorGraph::linearizeToHessianFactor
boost::shared_ptr< HessianFactor > linearizeToHessianFactor(const Values &values, boost::none_t, const Dampen &dampen=nullptr) const
Definition: NonlinearFactorGraph.h:255
gtsam::NonlinearFactorGraph::Dampen
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition: NonlinearFactorGraph.h:149
gtsam::GraphvizFormatting::figureWidthInches
double figureWidthInches
The figure width on paper in inches.
Definition: NonlinearFactorGraph.h:52
gtsam::NonlinearFactorGraph::addExpressionFactor
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:203
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:94
gtsam::GraphvizFormatting
Formatting options when saving in GraphViz format using NonlinearFactorGraph::saveGraph.
Definition: NonlinearFactorGraph.h:48
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition: Scatter.h:49
gtsam::KeyFormatter
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:35
gtsam::Expression
Expression class that supports automatic differentiation.
Definition: Expression.h:49
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
Point2.h
2D Point
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition: NonlinearFactorGraph.h:91
gtsam::GraphvizFormatting::binaryEdges
bool binaryEdges
just use non-dotted edges for binary factors
Definition: NonlinearFactorGraph.h:58
gtsam::GraphvizFormatting::scale
double scale
Scale all positions to reduce / increase density.
Definition: NonlinearFactorGraph.h:54
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::GraphvizFormatting::plotFactorPoints
bool plotFactorPoints
Plots each factor as a dot between the variables.
Definition: NonlinearFactorGraph.h:56
gtsam::GraphvizFormatting::figureHeightInches
double figureHeightInches
The figure height on paper in inches.
Definition: NonlinearFactorGraph.h:53
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition: ExpressionFactor.h:44
gtsam::GraphvizFormatting::paperVerticalAxis
Axis paperVerticalAxis
The world axis assigned to the vertical paper axis.
Definition: NonlinearFactorGraph.h:51
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition: make_shared.h:57