gtsam 4.2.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
29
30#include <boost/shared_ptr.hpp>
31#include <functional>
32
33namespace 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 boost::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
83
87
89 void print(
90 const std::string& str = "NonlinearFactorGraph: ",
91 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
92
94 void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
95 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
96 const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
97 printCondition = [](const Factor *,double, size_t) {return true;}) const;
98
100 bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
101
105
107 double error(const Values& values) const;
108
110 double probPrime(const Values& values) const;
111
115 boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
116
120 Ordering orderingCOLAMD() const;
121
130 Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
131
133 boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
134
136 typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
137
145 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
146 const Values& values, const Dampen& dampen = nullptr) const;
147
156 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
157 const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
158
161 Values updateCholesky(const Values& values,
162 const Dampen& dampen = nullptr) const;
163
166 Values updateCholesky(const Values& values, const Ordering& ordering,
167 const Dampen& dampen = nullptr) const;
168
170 NonlinearFactorGraph clone() const;
171
181 NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
182
189 template<typename T>
190 void addExpressionFactor(const SharedNoiseModel& R, const T& z,
191 const Expression<T>& h) {
192 push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
193 }
194
201 template<typename T>
202 void addPrior(Key key, const T& prior,
203 const SharedNoiseModel& model = nullptr) {
204 emplace_shared<PriorFactor<T>>(key, prior, model);
205 }
206
217 template<typename T>
218 void addPrior(Key key, const T& prior, const Matrix& covariance) {
219 emplace_shared<PriorFactor<T>>(key, prior, covariance);
220 }
221
225
226 using FactorGraph::dot;
228
230 void dot(std::ostream& os, const Values& values,
231 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
232 const GraphvizFormatting& writer = GraphvizFormatting()) const;
233
235 std::string dot(
236 const Values& values,
237 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
238 const GraphvizFormatting& writer = GraphvizFormatting()) const;
239
241 void saveGraph(
242 const std::string& filename, const Values& values,
243 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
244 const GraphvizFormatting& writer = GraphvizFormatting()) const;
246
247 private:
248
253 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
254 const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
255
257 friend class boost::serialization::access;
258 template<class ARCHIVE>
259 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
260 ar & boost::serialization::make_nvp("NonlinearFactorGraph",
261 boost::serialization::base_object<Base>(*this));
262 }
263
264 public:
265
266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
269
270 boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
271 const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
272 {return linearizeToHessianFactor(values, dampen);}
273
275 Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
276 const Dampen& dampen = nullptr) const
277 {return updateCholesky(values, dampen);}
278
280 void GTSAM_DEPRECATED saveGraph(
281 std::ostream& os, const Values& values = Values(),
282 const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
283 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
284 dot(os, values, keyFormatter, graphvizFormatting);
285 }
287 void GTSAM_DEPRECATED
288 saveGraph(const std::string& filename, const Values& values,
289 const GraphvizFormatting& graphvizFormatting,
290 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
291 saveGraph(filename, values, keyFormatter, graphvizFormatting);
292 }
294#endif
295
296 };
297
299template<>
300struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
301};
302
303} //\ namespace gtsam
304
2D Point
Factor Graph Base Class.
Graphviz formatter for NonlinearFactorGraph.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
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
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:195
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:97
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition: FactorGraph-inst.h:141
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition: FactorGraph-inst.h:177
Definition: Factor.h:68
Definition: Ordering.h:34
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition: Scatter.h:49
Factor that supports arbitrary expressions via AD.
Definition: ExpressionFactor.h:44
Expression class that supports automatic differentiation.
Definition: Expression.h:48
Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
Definition: GraphvizFormatting.h:32
Definition: NonlinearFactorGraph.h:55
NonlinearFactorGraph()
Default constructor.
Definition: NonlinearFactorGraph.h:67
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Convenience method which adds a PriorFactor to the factor graph.
Definition: NonlinearFactorGraph.h:202
NonlinearFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition: NonlinearFactorGraph.h:75
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:190
void addPrior(Key key, const T &prior, const Matrix &covariance)
Convenience method which adds a PriorFactor to the factor graph.
Definition: NonlinearFactorGraph.h:218
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition: NonlinearFactorGraph.h:71
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition: NonlinearFactorGraph.h:136
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition: NonlinearFactorGraph.h:79
virtual ~NonlinearFactorGraph()
Destructor.
Definition: NonlinearFactorGraph.h:82
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...