gtsam::NonlinearFactorGraph Class Reference
Detailed Description
A non-linear factor graph is a graph of non-Gaussian, i.e.
non-linear factors, which derive from NonlinearFactor. The values structures are typically (in SAM) more general than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. Linearizing the non-linear factor graph creates a linear factor graph on the tangent vector space at the linearization point. Because the tangent space is a true vector space, the config type will be an VectorValues in that linearized factor graph.
Inheritance diagram for gtsam::NonlinearFactorGraph:
Public Member Functions | |
| 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. | |
| void | print (const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
| print | |
| void | printErrors (const Values &values, const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
| print errors along with factors | |
| bool | equals (const NonlinearFactorGraph &other, double tol=1e-9) const |
| Test equality. | |
| void | saveGraph (std::ostream &stm, const Values &values=Values(), const GraphvizFormatting &graphvizFormatting=GraphvizFormatting(), const KeyFormatter &keyFormatter=DefaultKeyFormatter) const |
| Write the graph in GraphViz format for visualization. | |
| double | error (const Values &values) const |
| unnormalized error, \( 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \) in the most common case | |
| double | probPrime (const Values &values) const |
| Unnormalized probability. More... | |
| 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. More... | |
| boost::shared_ptr< GaussianFactorGraph > | linearize (const Values &linearizationPoint) const |
| Linearize a nonlinear factor graph. | |
| boost::shared_ptr< HessianFactor > | linearizeToHessianFactor (const Values &values, boost::optional< Ordering & > ordering=boost::none, const Dampen &dampen=nullptr) const |
| Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor. More... | |
| Values | updateCholesky (const Values &values, boost::optional< Ordering & > ordering=boost::none, const Dampen &dampen=nullptr) const |
| Linearize and solve in one pass. More... | |
| 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. More... | |
| 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. More... | |
| Public Member Functions inherited from gtsam::FactorGraph< NonlinearFactor > | |
| 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). | |
| std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value >::type | push_back (boost::shared_ptr< DERIVEDFACTOR > factor) |
| Add a factor directly using a shared_ptr. | |
| void | push_back (const sharedFactor &factor) |
| Add a factor directly using a shared_ptr. | |
| std::enable_if< std::is_base_of< FactorType, typename ITERATOR::value_type::element_type >::value >::type | push_back (ITERATOR firstFactor, ITERATOR lastFactor) |
| push back many factors with an iterator over shared_ptr (factors are not copied) | |
| std::enable_if< std::is_base_of< FactorType, typename CONTAINER::value_type::element_type >::value >::type | push_back (const CONTAINER &container) |
| push back many factors as shared_ptr's in a container (factors are not copied) | |
| std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type | push_back (const BayesTree< CLIQUE > &bayesTree) |
| push back a BayesTree as a collection of factors. More... | |
| std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value >::type | 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). More... | |
| std::enable_if< std::is_base_of< FactorType, typename ITERATOR::value_type >::value >::type | push_back (ITERATOR firstFactor, ITERATOR lastFactor) |
| push back many factors with an iterator over plain factors (factors are copied) | |
| std::enable_if< std::is_base_of< FactorType, typename CONTAINER::value_type >::value >::type | push_back (const CONTAINER &container) |
| push back many factors as non-pointer objects in a container (factors are copied) | |
| std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value >::type | emplace_shared (Args &&... args) |
| Emplace a factor. | |
| std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost::assign::list_inserter< RefCallPushBack< This > > >::type | operator+= (boost::shared_ptr< DERIVEDFACTOR > factor) |
| Add a factor directly using a shared_ptr. | |
| boost::assign::list_inserter< CRefCallPushBack< This > > | operator+= (const sharedFactor &factor) |
| Add a factor directly using a shared_ptr. | |
| boost::assign::list_inserter< CRefCallPushBack< This > > | operator+= (const FACTOR_OR_CONTAINER &factorOrContainer) |
| Add a factor or container of factors, including STL collections, BayesTrees, etc. More... | |
| std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value >::type | add (boost::shared_ptr< DERIVEDFACTOR > factor) |
| Add a factor directly using a shared_ptr. | |
| void | add (const sharedFactor &factor) |
| Add a factor directly using a shared_ptr. | |
| void | add (const FACTOR_OR_CONTAINER &factorOrContainer) |
| Add a factor or container of factors, including STL collections, BayesTrees, etc. More... | |
| void | print (const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const |
| print out graph | |
| bool | equals (const This &fg, double tol=1e-9) const |
| Check equality. More... | |
| size_t | size () const |
| return the number of factors (including any null factors set by remove() ). More... | |
| bool | empty () const |
| Check if the graph is empty (null factors set by remove() will cause this to return false). More... | |
| 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. More... | |
| const_iterator | end () const |
| Iterator to end of factors. More... | |
| sharedFactor | front () const |
| Get the first factor. | |
| sharedFactor | back () const |
| Get the last factor. | |
| iterator | begin () |
| non-const STL-style begin() | |
| iterator | end () |
| non-const STL-style end() | |
| void | resize (size_t size) |
| Directly resize the number of factors in the graph. More... | |
| void | remove (size_t i) |
| delete factor without re-arranging indexes by inserting a NULL 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. | |
| 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 |
| typedef std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> | Dampen |
| typdef for dampen functions used below | |
| 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 > | |
| 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 More... | |
Member Function Documentation
◆ addExpressionFactor()
template<typename T >
|
inline |
Directly add ExpressionFactor that implements |h(x)-z|^2_R.
- Parameters
-
h expression that implements measurement function z measurement R model
◆ linearizeToHessianFactor()
| HessianFactor::shared_ptr gtsam::NonlinearFactorGraph::linearizeToHessianFactor | ( | const Values & | values, |
| boost::optional< Ordering & > | ordering = boost::none, |
||
| 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 ordering can be 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
◆ 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()
| Values gtsam::NonlinearFactorGraph::updateCholesky | ( | const Values & | values, |
| boost::optional< Ordering & > | ordering = boost::none, |
||
| 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:
- /Users/dellaert/git/gtsam/gtsam/nonlinear/NonlinearFactorGraph.h
- /Users/dellaert/git/gtsam/gtsam/nonlinear/NonlinearFactorGraph.cpp
