GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
Public Types | |
typedef FactorGraph< NonlinearFactor > | Base |
typedef NonlinearFactorGraph | This |
typedef std::shared_ptr< This > | shared_ptr |
typedef NonlinearFactor | FactorType |
factor type | |
typedef std::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 |
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. | |
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. | |
Adding Single Factors | |
void | reserve (size_t size) |
IsDerived< DERIVEDFACTOR > | push_back (std::shared_ptr< DERIVEDFACTOR > factor) |
Add a factor directly using a shared_ptr. | |
IsDerived< DERIVEDFACTOR > | push_back (const DERIVEDFACTOR &factor) |
IsDerived< DERIVEDFACTOR > | emplace_shared (Args &&... args) |
Emplace a shared pointer to factor of given type. | |
IsDerived< DERIVEDFACTOR > | add (std::shared_ptr< DERIVEDFACTOR > factor) |
add is a synonym for push_back. | |
Adding via iterators | |
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) | |
Adding via container | |
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). | |
void | add (const FACTOR_OR_CONTAINER &factorOrContainer) |
Specialized versions | |
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) |
Testable | |
bool | equals (const This &fg, double tol=1e-9) const |
Check equality up to tolerance. | |
Standard Interface | |
size_t | size () const |
bool | empty () const |
const sharedFactor | at (size_t i) const |
sharedFactor & | at (size_t i) |
const sharedFactor | operator[] (size_t i) const |
sharedFactor & | operator[] (size_t i) |
const_iterator | begin () const |
const_iterator | end () const |
sharedFactor | front () const |
sharedFactor | back () const |
double | error (const HybridValues &values) const |
Modifying Factor Graphs (imperative, discouraged) | |
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) |
Graph Display | |
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. | |
Advanced Interface | |
size_t | nrFactors () const |
KeySet | keys () const |
KeyVector | keyVector () const |
bool | exists (size_t idx) const |
Protected Member Functions | |
bool | isEqual (const FactorGraph &other) const |
Check exact equality of the factor pointers. Useful for derived ==. | |
Protected Attributes | |
FastVector< sharedFactor > | factors_ |
Standard Interface | |
typedef std::function< void(const std::shared_ptr< HessianFactor > &hessianFactor)> | Dampen |
typdef for dampen functions used below | |
double | error (const Values &values) const |
double | probPrime (const Values &values) const |
std::shared_ptr< SymbolicFactorGraph > | symbolic () const |
Ordering | orderingCOLAMD () const |
Ordering | orderingCOLAMDConstrained (const FastMap< Key, int > &constraints) const |
std::shared_ptr< GaussianFactorGraph > | linearize (const Values &linearizationPoint) const |
Linearize a nonlinear factor graph. | |
std::shared_ptr< HessianFactor > | linearizeToHessianFactor (const Values &values, const Dampen &dampen=nullptr) const |
std::shared_ptr< HessianFactor > | linearizeToHessianFactor (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. | |
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) |
|
inline |
Default constructor
|
inline |
Construct from iterator over factors
|
inlineexplicit |
Construct from container of factors (shared_ptr or plain objects)
|
inline |
Implicit copy/downcast constructor to override explicit template container constructor
|
inlineinherited |
Add a factor or container of factors, including STL collections, BayesTrees, etc.
|
inherited |
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and reusing empty factor slots.
|
inline |
Directly add ExpressionFactor that implements |h(x)-z|^2_R
h | expression that implements measurement function |
z | measurement |
R | model |
|
inline |
Convenience method which adds a PriorFactor to the factor graph.
key | Variable key |
prior | The variable's prior value |
model | Noise model for prior factor |
|
inline |
Convenience method which adds a PriorFactor to the factor graph.
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).
|
inlineinherited |
Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).
|
inlineinherited |
Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).
|
inlineinherited |
Get the last factor
|
inlineinherited |
Iterator to beginning of factors.
|
inlineinherited |
non-const STL-style begin()
|
inlineinherited |
Check if the graph is empty (null factors set by remove() will cause this to return false).
|
inlineinherited |
Iterator to end of factors.
|
inlineinherited |
non-const STL-style end()
bool gtsam::NonlinearFactorGraph::equals | ( | const NonlinearFactorGraph & | other, |
double | tol = 1e-9 |
||
) | const |
Test equality
|
inlineinherited |
Erase factor and rearrange other factors to take up the empty space
|
inlineinherited |
Erase factors and rearrange other factors to take up the empty space
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
|
inherited |
Add error for all factors.
|
inlineinherited |
MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer
|
inlineinherited |
Get the first factor
|
inherited |
Potentially slow function to return all keys involved, sorted, as a set
|
inherited |
Potentially slow function to return all keys involved, sorted, as a vector
std::shared_ptr<HessianFactor> 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.
std::shared_ptr<HessianFactor> 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.
|
inherited |
return the number of non-null factors
|
inlineinherited |
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
|
inlineinherited |
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
Ordering gtsam::NonlinearFactorGraph::orderingCOLAMD | ( | ) | const |
Compute a fill-reducing ordering using COLAMD.
Ordering gtsam::NonlinearFactorGraph::orderingCOLAMDConstrained | ( | const FastMap< Key, int > & | constraints | ) | const |
Compute a fill-reducing ordering with constraints using CCOLAMD
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 |
|
overridevirtual |
Reimplemented from gtsam::FactorGraph< NonlinearFactor >.
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
double gtsam::NonlinearFactorGraph::probPrime | ( | const Values & | values | ) | const |
Unnormalized probability. O(n)
|
inlineinherited |
Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).
|
inlineinherited |
Push back many factors with an iterator over shared_ptr (factors are not copied)
|
inlineinherited |
Push back many factors as shared_ptr's in a container (factors are not copied)
|
inlineinherited |
Push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived classes in favor of a type-specialized version that calls this templated function.
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.
rekey_mapping | is a map of old->new keys |
|
inlineinherited |
delete factor without re-arranging indexes by inserting a nullptr pointer
|
inlineinherited |
replace a factor by index
|
inlineinherited |
Reserve space for the specified number of factors if you know in advance how many there will be (works like FastVector::reserve).
|
inlinevirtualinherited |
Directly resize the number of factors in the graph. If the new size is less than the original, factors at the end will be removed. If the new size is larger than the original, null factors will be appended.
|
inlineinherited |
return the number of factors (including any null factors set by remove() ).
std::shared_ptr<SymbolicFactorGraph> gtsam::NonlinearFactorGraph::symbolic | ( | ) | const |
Create a symbolic factor graph
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.
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.
|
protectedinherited |
concept check, makes sure FACTOR defines print and equals Collection of factors