30#include <boost/shared_ptr.hpp>
38 class GaussianFactorGraph;
39 class SymbolicFactorGraph;
43 class ExpressionFactor;
61 typedef boost::shared_ptr<This> shared_ptr;
70 template<
typename ITERATOR>
74 template<
class CONTAINER>
78 template<
class DERIVEDFACTOR>
90 const std::string& str =
"NonlinearFactorGraph: ",
91 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override;
94 void printErrors(
const Values& values,
const std::string& str =
"NonlinearFactorGraph: ",
96 const std::function<
bool(
const Factor* ,
double ,
size_t )>&
97 printCondition = [](
const Factor *,
double,
size_t) {
return true;})
const;
100 bool equals(
const NonlinearFactorGraph& other,
double tol = 1e-9)
const;
107 double error(
const Values& values)
const;
110 double probPrime(
const Values& values)
const;
115 boost::shared_ptr<SymbolicFactorGraph> symbolic()
const;
120 Ordering orderingCOLAMD()
const;
130 Ordering orderingCOLAMDConstrained(
const FastMap<Key, int>& constraints)
const;
133 boost::shared_ptr<GaussianFactorGraph> linearize(
const Values& linearizationPoint)
const;
136 typedef std::function<void(
const boost::shared_ptr<HessianFactor>& hessianFactor)>
Dampen;
145 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
146 const Values& values,
const Dampen& dampen =
nullptr)
const;
156 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
162 const Dampen& dampen =
nullptr)
const;
167 const Dampen& dampen =
nullptr)
const;
204 emplace_shared<PriorFactor<T>>(key, prior, model);
218 void addPrior(
Key key,
const T& prior,
const Matrix& covariance) {
219 emplace_shared<PriorFactor<T>>(key, prior, covariance);
230 void dot(std::ostream& os,
const Values& values,
242 const std::string& filename,
const Values& values,
253 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
254 const Values& values,
const Scatter& scatter,
const Dampen& dampen =
nullptr)
const;
257 friend class boost::serialization::access;
258 template<
class ARCHIVE>
259 void serialize(ARCHIVE & ar,
const unsigned int ) {
260 ar & boost::serialization::make_nvp(
"NonlinearFactorGraph",
261 boost::serialization::base_object<Base>(*
this));
266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
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);}
275 Values GTSAM_DEPRECATED updateCholesky(
const Values& values, boost::none_t,
276 const Dampen& dampen =
nullptr)
const
277 {
return updateCholesky(values, dampen);}
280 void GTSAM_DEPRECATED saveGraph(
282 const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
283 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const {
284 dot(os, values, keyFormatter, graphvizFormatting);
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);
Non-linear factor base classes.
Graphviz formatter for NonlinearFactorGraph.
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: 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...