Loading...
Searching...
No Matches
gtsam::NonlinearFactorGraph Class Reference
Inheritance diagram for gtsam::NonlinearFactorGraph:

## Standard Interface

typedef std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> | Dampen
| typdef for dampen functions used below
double | error (const Values &values) const
| unnormalized error, \( \sum_i 0.5 (h_i(X_i)-z)^2 / \sigma^2 \) in the most common case
double | probPrime (const Values &values) const
| Unnormalized probability.
boost::shared_ptr< SymbolicFactorGraph > | symbolic () const
| Create a symbolic factor graph.
Ordering | orderingCOLAMD () const
| Compute a fill-reducing ordering using COLAMD.
Ordering | orderingCOLAMDConstrained (const FastMap< Key, int > &constraints) const
| Compute a fill-reducing ordering with constraints using CCOLAMD.
boost::shared_ptr< GaussianFactorGraph > | linearize (const Values &linearizationPoint) const
| Linearize a nonlinear factor graph.
boost::shared_ptr< HessianFactor > | linearizeToHessianFactor (const Values &values, const Dampen &dampen=nullptr) const
| Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor.
boost::shared_ptr< HessianFactor > | 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.
Values | updateCholesky (const Values &values, const Dampen &dampen=nullptr) const
| Linearize and solve in one pass.
Values | updateCholesky (const Values &values, const Ordering &ordering, const Dampen &dampen=nullptr) const
| Linearize and solve in one pass.
NonlinearFactorGraph | clone () const
| Clone() performs a deep-copy of the graph, including all of the factors.
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.
template<typename T>
void | addExpressionFactor (const SharedNoiseModel &R, const T &z, const Expression< T > &h)
| Directly add ExpressionFactor that implements |h(x)-z|^2_R.
template<typename T>
void | addPrior (Key key, const T &prior, const SharedNoiseModel &model=nullptr)
| Convenience method which adds a PriorFactor to the factor graph.
template<typename T>
void | addPrior (Key key, const T &prior, const Matrix &covariance)
| Convenience method which adds a PriorFactor to the factor graph.

## Public Member Functions

Standard Constructors
| NonlinearFactorGraph ()
| Default constructor.
template<typename ITERATOR>
| NonlinearFactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
| Construct from iterator over factors.
template<class CONTAINER>
| NonlinearFactorGraph (const CONTAINER &factors)
| Construct from container of factors (shared_ptr or plain objects).
template<class DERIVEDFACTOR>
| NonlinearFactorGraph (const FactorGraph< DERIVEDFACTOR > &graph)
| Implicit copy/downcast constructor to override explicit template container constructor.
virtual | ~NonlinearFactorGraph ()
| Destructor.
Testable
void | print (const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
| print
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
| print errors along with factors
bool | equals (const NonlinearFactorGraph &other, double tol=1e-9) const
| Test equality.
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.
std::string | dot (const Values &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const GraphvizFormatting &writer=GraphvizFormatting()) const
| Output to graphviz format string, with Values/extra options.
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.
void | dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
| Output to graphviz format, stream version.
std::string | dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
| Output to graphviz format string.
void | saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
| output to file with graphviz format.
Public Member Functions inherited from gtsam::FactorGraph< NonlinearFactor >
virtual | ~FactorGraph ()=default
| Default destructor Public and virtual so boost serialization can call it.
| FactorGraph (std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > sharedFactors)
| Constructor that takes an initializer list of shared pointers.
void | reserve (size_t size)
| Reserve space for the specified number of factors if you know in advance how many there will be (works like FastVector::reserve).
IsDerived< DERIVEDFACTOR > | push_back (boost::shared_ptr< DERIVEDFACTOR > factor)
| Add a factor directly using a shared_ptr.
IsDerived< DERIVEDFACTOR > | emplace_shared (Args &&... args)
| Emplace a shared pointer to factor of given type.
IsDerived< DERIVEDFACTOR > | push_back (const DERIVEDFACTOR &factor)
| Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).
IsDerived< DERIVEDFACTOR > | add (boost::shared_ptr< DERIVEDFACTOR > factor)
| add is a synonym for push_back.
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost::assign::list_inserter< RefCallPushBack< This > > >::type | operator+= (boost::shared_ptr< DERIVEDFACTOR > factor)
| += works well with boost::assign list inserter.
HasDerivedElementType< ITERATOR > | push_back (ITERATOR firstFactor, ITERATOR lastFactor)
| Push back many factors with an iterator over shared_ptr (factors are not copied).
HasDerivedValueType< ITERATOR > | push_back (ITERATOR firstFactor, ITERATOR lastFactor)
| Push back many factors with an iterator (factors are copied).
HasDerivedElementType< CONTAINER > | push_back (const CONTAINER &container)
| Push back many factors as shared_ptr's in a container (factors are not copied).
HasDerivedValueType< CONTAINER > | push_back (const CONTAINER &container)
| Push back non-pointer objects in a container (factors are copied).
void | add (const FACTOR_OR_CONTAINER &factorOrContainer)
| Add a factor or container of factors, including STL collections, BayesTrees, etc.
boost::assign::list_inserter< CRefCallPushBack< This > > | operator+= (const FACTOR_OR_CONTAINER &factorOrContainer)
| Add a factor or container of factors, including STL collections, BayesTrees, etc.
std::enable_if< std::is_base_of< This, typenameCLIQUE::FactorGraphType >::value >::type | push_back (const BayesTree< CLIQUE > &bayesTree)
| Push back a BayesTree as a collection of factors.
FactorIndices | add_factors (const CONTAINER &factors, bool useEmptySlots=false)
| Add new factors to a factor graph and returns a list of new factor indices, optionally finding and reusing empty factor slots.
bool | equals (const This &fg, double tol=1e-9) const
| Check equality up to tolerance.
size_t | size () const
| return the number of factors (including any null factors set by remove() ).
bool | empty () const
| Check if the graph is empty (null factors set by remove() will cause this to return false).
const sharedFactor | at (size_t i) const
| Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).
sharedFactor & | at (size_t i)
| Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).
const sharedFactor | operator[] (size_t i) const
| Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
sharedFactor & | operator[] (size_t i)
| Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
const_iterator | begin () const
| Iterator to beginning of factors.
const_iterator | end () const
| Iterator to end of factors.
sharedFactor | front () const
| Get the first factor.
sharedFactor | back () const
| Get the last factor.
double | error (const HybridValues &values) const
| Add error for all factors.
iterator | begin ()
| non-const STL-style begin()
iterator | end ()
| non-const STL-style end()
virtual void | resize (size_t size)
| Directly resize the number of factors in the graph.
void | remove (size_t i)
| delete factor without re-arranging indexes by inserting a nullptr pointer
void | replace (size_t index, sharedFactor factor)
| replace a factor by index
iterator | erase (iterator item)
| Erase factor and rearrange other factors to take up the empty space.
iterator | erase (iterator first, iterator last)
| Erase factors and rearrange other factors to take up the empty space.
void | dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
| Output to graphviz format, stream version.
std::string | dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
| Output to graphviz format string.
void | saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
| output to file with graphviz format.
size_t | nrFactors () const
| return the number of non-null factors
KeySet | keys () const
| Potentially slow function to return all keys involved, sorted, as a set.
KeyVector | keyVector () const
| Potentially slow function to return all keys involved, sorted, as a vector.
bool | exists (size_t idx) const
| MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer.

## Public Types

typedef FactorGraph< NonlinearFactor > | Base
typedef NonlinearFactorGraph | This
typedef boost::shared_ptr< This > | shared_ptr
Public Types inherited from gtsam::FactorGraph< NonlinearFactor >
typedef NonlinearFactor | FactorType
| factor type
typedef boost::shared_ptr< NonlinearFactor > | sharedFactor
| Shared pointer to a factor.
typedef sharedFactor | value_type
typedef FastVector< sharedFactor >::iterator | iterator
typedef FastVector< sharedFactor >::const_iterator | const_iterator

## Friends

class | boost::serialization::access
| Serialization function.

## 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 ==.
| FactorGraph ()
| Default constructor.
| FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
| Constructor from iterator over factors (shared_ptr or plain objects).
| FactorGraph (const CONTAINER &factors)
| Construct from container of factors (shared_ptr or plain objects).
Protected Attributes inherited from gtsam::FactorGraph< NonlinearFactor >
FastVector< sharedFactor > | factors_
| concept check, makes sure FACTOR defines print and equals

## 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

  h | expression that implements measurement function
  z | measurement
  R | model

## 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

  key | Variable key
  prior | The variable's prior value
  covariance | Covariance 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).

## 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

  key | Variable key
  prior | The variable's prior value
  model | Noise model for prior factor

## linearizeToHessianFactor() [1/2]

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.

## linearizeToHessianFactor() [2/2]

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.

## orderingCOLAMDConstrained()

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

Compute a fill-reducing ordering with constraints using CCOLAMD.

Parameters

  constraints | is 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

## print()

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

override virtual

print

Reimplemented from gtsam::FactorGraph< NonlinearFactor >.

## probPrime()

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

Unnormalized probability.

O(n)

## 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_mapping | is a map of old->new keys

Returns
  a cloned graph with updated keys

## 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.

## 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.

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

- /build/gtsam-gchP9J/gtsam-4.2.2+dfsg/gtsam/nonlinear/NonlinearFactorGraph.h
- /build/gtsam-gchP9J/gtsam-4.2.2+dfsg/gtsam/nonlinear/NonlinearFactorGraph.cpp