Public Types | Private Member Functions | List of all members
gtsam::NonlinearFactorGraph Class Reference

#include <NonlinearFactorGraph.h>

Inheritance diagram for gtsam::NonlinearFactorGraph:
Inheritance graph
[legend]

Public Types

typedef FactorGraph< NonlinearFactorBase
 
typedef std::shared_ptr< Thisshared_ptr
 
typedef NonlinearFactorGraph This
 
- Public Types inherited from gtsam::FactorGraph< NonlinearFactor >
typedef FastVector< sharedFactor >::const_iterator const_iterator
 
typedef NonlinearFactor FactorType
 factor type More...
 
typedef FastVector< sharedFactor >::iterator iterator
 
typedef std::shared_ptr< NonlinearFactorsharedFactor
 Shared pointer to a factor. More...
 
typedef sharedFactor value_type
 

Public Member Functions

Standard Constructors
 NonlinearFactorGraph ()
 
template<typename ITERATOR >
 NonlinearFactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
 
template<class CONTAINER >
 NonlinearFactorGraph (const CONTAINER &factors)
 
template<class DERIVEDFACTOR >
 NonlinearFactorGraph (const FactorGraph< DERIVEDFACTOR > &graph)
 
Testable
void print (const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
void printErrors (const Values &values, const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter, const std::function< bool(const Factor *, double, size_t)> &printCondition=[](const Factor *, double, size_t) {return true;}) const
 
bool equals (const NonlinearFactorGraph &other, double tol=1e-9) const
 
Graph Display
void dot (std::ostream &os, const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
 Output to graphviz format, stream version, with Values/extra options. More...
 
std::string dot (const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
 Output to graphviz format string, with Values/extra options. More...
 
void saveGraph (const std::string &filename, const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
 output to file with graphviz format, with Values/extra options. More...
 
void dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format, stream version. More...
 
std::string dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format string. More...
 
void saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 output to file with graphviz format. More...
 
- Public Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
 FactorGraph (std::initializer_list< std::shared_ptr< DERIVEDFACTOR >> sharedFactors)
 
virtual ~FactorGraph ()=default
 
void reserve (size_t size)
 
IsDerived< DERIVEDFACTOR > push_back (std::shared_ptr< DERIVEDFACTOR > factor)
 Add a factor directly using a shared_ptr. More...
 
IsDerived< DERIVEDFACTOR > push_back (const DERIVEDFACTOR &factor)
 
IsDerived< DERIVEDFACTOR > emplace_shared (Args &&... args)
 Emplace a shared pointer to factor of given type. More...
 
IsDerived< DERIVEDFACTOR > add (std::shared_ptr< DERIVEDFACTOR > factor)
 add is a synonym for push_back. More...
 
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::typeoperator+= (std::shared_ptr< DERIVEDFACTOR > factor)
 Append factor to factor graph. More...
 
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::typeoperator, (std::shared_ptr< DERIVEDFACTOR > factor)
 Overload comma operator to allow for append chaining. More...
 
HasDerivedElementType< ITERATOR > push_back (ITERATOR firstFactor, ITERATOR lastFactor)
 
HasDerivedValueType< ITERATOR > push_back (ITERATOR firstFactor, ITERATOR lastFactor)
 Push back many factors with an iterator (factors are copied) More...
 
HasDerivedElementType< CONTAINER > push_back (const CONTAINER &container)
 
HasDerivedValueType< CONTAINER > push_back (const CONTAINER &container)
 Push back non-pointer objects in a container (factors are copied). More...
 
void add (const FACTOR_OR_CONTAINER &factorOrContainer)
 
Thisoperator+= (const FACTOR_OR_CONTAINER &factorOrContainer)
 
std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type push_back (const BayesTree< CLIQUE > &bayesTree)
 
FactorIndices add_factors (const CONTAINER &factors, bool useEmptySlots=false)
 
bool equals (const This &fg, double tol=1e-9) const
 Check equality up to tolerance. More...
 
size_t size () const
 
bool empty () const
 
const sharedFactor at (size_t i) const
 
sharedFactorat (size_t i)
 
std::shared_ptr< F > at (size_t i)
 
const std::shared_ptr< F > at (size_t i) const
 Const version of templated at method. More...
 
const sharedFactor operator[] (size_t i) const
 
sharedFactoroperator[] (size_t i)
 
const_iterator begin () const
 
const_iterator end () const
 
sharedFactor front () const
 
sharedFactor back () const
 
double error (const HybridValues &values) const
 
iterator begin ()
 
iterator end ()
 
virtual void resize (size_t size)
 
void remove (size_t i)
 
void replace (size_t index, sharedFactor factor)
 
iterator erase (iterator item)
 
iterator erase (iterator first, iterator last)
 
void dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format, stream version. More...
 
std::string dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format string. More...
 
void saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 output to file with graphviz format. More...
 
size_t nrFactors () const
 
KeySet keys () const
 
KeyVector keyVector () const
 
bool exists (size_t idx) const
 

Private Member Functions

std::shared_ptr< HessianFactorlinearizeToHessianFactor (const Values &values, const Scatter &scatter, const Dampen &dampen=nullptr) const
 

Standard Interface

typedef std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)> Dampen
 typdef for dampen functions used below More...
 
double error (const Values &values) const
 
double probPrime (const Values &values) const
 
std::shared_ptr< SymbolicFactorGraphsymbolic () const
 
Ordering orderingCOLAMD () const
 
Ordering orderingCOLAMDConstrained (const FastMap< Key, int > &constraints) const
 
std::shared_ptr< GaussianFactorGraphlinearize (const Values &linearizationPoint) const
 Linearize a nonlinear factor graph. More...
 
std::shared_ptr< HessianFactorlinearizeToHessianFactor (const Values &values, const Dampen &dampen=nullptr) const
 
std::shared_ptr< HessianFactorlinearizeToHessianFactor (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const
 
Values updateCholesky (const Values &values, const Dampen &dampen=nullptr) const
 
Values updateCholesky (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const
 
NonlinearFactorGraph clone () const
 Clone() performs a deep-copy of the graph, including all of the factors. More...
 
NonlinearFactorGraph rekey (const std::map< Key, Key > &rekey_mapping) const
 
template<typename T >
void addExpressionFactor (const SharedNoiseModel &R, const T &z, const Expression< T > &h)
 
template<typename T >
void addPrior (Key key, const T &prior, const SharedNoiseModel &model=nullptr)
 
template<typename T >
void addPrior (Key key, const T &prior, const Matrix &covariance)
 

Additional Inherited Members

- Protected Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
bool isEqual (const FactorGraph &other) const
 Check exact equality of the factor pointers. Useful for derived ==. More...
 
 FactorGraph ()
 
 FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
 
 FactorGraph (const CONTAINER &factors)
 
- Protected Attributes inherited from gtsam::FactorGraph< NonlinearFactor >
FastVector< sharedFactorfactors_
 

Detailed Description

Definition at line 55 of file NonlinearFactorGraph.h.

Member Typedef Documentation

◆ Base

Definition at line 59 of file NonlinearFactorGraph.h.

◆ Dampen

typedef std::function<void(const std::shared_ptr<HessianFactor>& hessianFactor)> gtsam::NonlinearFactorGraph::Dampen

typdef for dampen functions used below

Definition at line 133 of file NonlinearFactorGraph.h.

◆ shared_ptr

Definition at line 61 of file NonlinearFactorGraph.h.

◆ This

Definition at line 60 of file NonlinearFactorGraph.h.

Constructor & Destructor Documentation

◆ NonlinearFactorGraph() [1/4]

gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( )
inline

Default constructor

Definition at line 67 of file NonlinearFactorGraph.h.

◆ NonlinearFactorGraph() [2/4]

template<typename ITERATOR >
gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( ITERATOR  firstFactor,
ITERATOR  lastFactor 
)
inline

Construct from iterator over factors

Definition at line 71 of file NonlinearFactorGraph.h.

◆ NonlinearFactorGraph() [3/4]

template<class CONTAINER >
gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( const CONTAINER &  factors)
inlineexplicit

Construct from container of factors (shared_ptr or plain objects)

Definition at line 75 of file NonlinearFactorGraph.h.

◆ NonlinearFactorGraph() [4/4]

template<class DERIVEDFACTOR >
gtsam::NonlinearFactorGraph::NonlinearFactorGraph ( const FactorGraph< DERIVEDFACTOR > &  graph)
inline

Implicit copy/downcast constructor to override explicit template container constructor

Definition at line 79 of file NonlinearFactorGraph.h.

Member Function Documentation

◆ addExpressionFactor()

template<typename T >
void gtsam::NonlinearFactorGraph::addExpressionFactor ( const SharedNoiseModel R,
const T z,
const Expression< T > &  h 
)
inline

Directly add ExpressionFactor that implements |h(x)-z|^2_R

Parameters
hexpression that implements measurement function
zmeasurement
Rmodel

Definition at line 187 of file NonlinearFactorGraph.h.

◆ addPrior() [1/2]

template<typename T >
void gtsam::NonlinearFactorGraph::addPrior ( Key  key,
const T prior,
const Matrix covariance 
)
inline

Convenience method which adds a PriorFactor to the factor graph.

Parameters
keyVariable key
priorThe variable's prior value
covarianceCovariance matrix.

Note that the smart noise model associated with the prior factor automatically picks the right noise model (e.g. a diagonal noise model if the provided covariance matrix is diagonal).

Definition at line 215 of file NonlinearFactorGraph.h.

◆ addPrior() [2/2]

template<typename T >
void gtsam::NonlinearFactorGraph::addPrior ( Key  key,
const T prior,
const SharedNoiseModel model = nullptr 
)
inline

Convenience method which adds a PriorFactor to the factor graph.

Parameters
keyVariable key
priorThe variable's prior value
modelNoise model for prior factor

Definition at line 199 of file NonlinearFactorGraph.h.

◆ clone()

NonlinearFactorGraph gtsam::NonlinearFactorGraph::clone ( ) const

Clone() performs a deep-copy of the graph, including all of the factors.

Definition at line 372 of file NonlinearFactorGraph.cpp.

◆ dot() [1/4]

std::string gtsam::FactorGraph< FACTOR >::dot

Output to graphviz format string.

Definition at line 168 of file FactorGraph-inst.h.

◆ dot() [2/4]

std::string gtsam::NonlinearFactorGraph::dot ( const Values values,
const KeyFormatter keyFormatter = DefaultKeyFormatter,
const GraphvizFormatting writer = GraphvizFormatting() 
) const

Output to graphviz format string, with Values/extra options.

Definition at line 151 of file NonlinearFactorGraph.cpp.

◆ dot() [3/4]

void gtsam::FactorGraph< FACTOR >::dot

Output to graphviz format, stream version.

Definition at line 141 of file FactorGraph-inst.h.

◆ dot() [4/4]

void gtsam::NonlinearFactorGraph::dot ( std::ostream &  os,
const Values values,
const KeyFormatter keyFormatter = DefaultKeyFormatter,
const GraphvizFormatting writer = GraphvizFormatting() 
) const

Output to graphviz format, stream version, with Values/extra options.

Definition at line 102 of file NonlinearFactorGraph.cpp.

◆ equals()

bool gtsam::NonlinearFactorGraph::equals ( const NonlinearFactorGraph other,
double  tol = 1e-9 
) const

Test equality

Definition at line 97 of file NonlinearFactorGraph.cpp.

◆ error()

double gtsam::NonlinearFactorGraph::error ( const Values values) const

unnormalized error, $ \sum_i 0.5 (h_i(X_i)-z)^2 / \sigma^2 $ in the most common case

Definition at line 170 of file NonlinearFactorGraph.cpp.

◆ linearize()

GaussianFactorGraph::shared_ptr gtsam::NonlinearFactorGraph::linearize ( const Values linearizationPoint) const

Linearize a nonlinear factor graph.

Definition at line 239 of file NonlinearFactorGraph.cpp.

◆ linearizeToHessianFactor() [1/3]

HessianFactor::shared_ptr gtsam::NonlinearFactorGraph::linearizeToHessianFactor ( const Values values,
const Dampen dampen = nullptr 
) const

Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing a new graph, and hence useful in case a dense solve is appropriate for your problem. An optional lambda function can be used to apply damping on the filled Hessian. No parallelism is exploited, because all the factors write in the same memory.

Definition at line 344 of file NonlinearFactorGraph.cpp.

◆ linearizeToHessianFactor() [2/3]

HessianFactor::shared_ptr gtsam::NonlinearFactorGraph::linearizeToHessianFactor ( const Values values,
const Ordering ordering,
const Dampen dampen = nullptr 
) const

Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing a new graph, and hence useful in case a dense solve is appropriate for your problem. An ordering is given that still decides how the Hessian is laid out. An optional lambda function can be used to apply damping on the filled Hessian. No parallelism is exploited, because all the factors write in the same memory.

Definition at line 335 of file NonlinearFactorGraph.cpp.

◆ linearizeToHessianFactor() [3/3]

HessianFactor::shared_ptr gtsam::NonlinearFactorGraph::linearizeToHessianFactor ( const Values values,
const Scatter scatter,
const Dampen dampen = nullptr 
) const
private

Linearize from Scatter rather than from Ordering. Made private because it doesn't include gttic.

Definition at line 312 of file NonlinearFactorGraph.cpp.

◆ orderingCOLAMD()

Ordering gtsam::NonlinearFactorGraph::orderingCOLAMD ( ) const

Compute a fill-reducing ordering using COLAMD.

Definition at line 182 of file NonlinearFactorGraph.cpp.

◆ orderingCOLAMDConstrained()

Ordering gtsam::NonlinearFactorGraph::orderingCOLAMDConstrained ( const FastMap< Key, int > &  constraints) const

Compute a fill-reducing ordering with constraints using CCOLAMD

Parameters
constraintsis a map of Key->group, where 0 is unconstrained, and higher group numbers are further back in the ordering. Only keys with nonzero group indices need to appear in the constraints, unconstrained is assumed for all other variables

Definition at line 188 of file NonlinearFactorGraph.cpp.

◆ print()

void gtsam::NonlinearFactorGraph::print ( const std::string &  str = "NonlinearFactorGraph: ",
const KeyFormatter keyFormatter = DefaultKeyFormatter 
) const
overridevirtual

print

Reimplemented from gtsam::FactorGraph< NonlinearFactor >.

Definition at line 55 of file NonlinearFactorGraph.cpp.

◆ printErrors()

void gtsam::NonlinearFactorGraph::printErrors ( const Values values,
const std::string &  str = "NonlinearFactorGraph: ",
const KeyFormatter keyFormatter = DefaultKeyFormatter,
const std::function< bool(const Factor *, double, size_t)> &  printCondition = [](const Factor *,double, size_t) {return true;} 
) const

print errors along with factors

Definition at line 71 of file NonlinearFactorGraph.cpp.

◆ probPrime()

double gtsam::NonlinearFactorGraph::probPrime ( const Values values) const

Unnormalized probability. O(n)

Definition at line 49 of file NonlinearFactorGraph.cpp.

◆ rekey()

NonlinearFactorGraph gtsam::NonlinearFactorGraph::rekey ( const std::map< Key, Key > &  rekey_mapping) const

Rekey() performs a deep-copy of all of the factors, and changes keys according to a mapping.

Keys not specified in the mapping will remain unchanged.

Parameters
rekey_mappingis a map of old->new keys
Returns
a cloned graph with updated keys

Definition at line 384 of file NonlinearFactorGraph.cpp.

◆ saveGraph() [1/2]

void gtsam::FactorGraph< FACTOR >::saveGraph

output to file with graphviz format.

Definition at line 177 of file FactorGraph-inst.h.

◆ saveGraph() [2/2]

void gtsam::NonlinearFactorGraph::saveGraph ( const std::string &  filename,
const Values values,
const KeyFormatter keyFormatter = DefaultKeyFormatter,
const GraphvizFormatting writer = GraphvizFormatting() 
) const

output to file with graphviz format, with Values/extra options.

Definition at line 160 of file NonlinearFactorGraph.cpp.

◆ symbolic()

SymbolicFactorGraph::shared_ptr gtsam::NonlinearFactorGraph::symbolic ( ) const

Create a symbolic factor graph

Definition at line 194 of file NonlinearFactorGraph.cpp.

◆ updateCholesky() [1/2]

Values gtsam::NonlinearFactorGraph::updateCholesky ( const Values values,
const Dampen dampen = nullptr 
) const

Linearize and solve in one pass. Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.

Definition at line 353 of file NonlinearFactorGraph.cpp.

◆ updateCholesky() [2/2]

Values gtsam::NonlinearFactorGraph::updateCholesky ( const Values values,
const Ordering ordering,
const Dampen dampen = nullptr 
) const

Linearize and solve in one pass. Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.

Definition at line 362 of file NonlinearFactorGraph.cpp.


The documentation for this class was generated from the following files:


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:52:39