#include <NonlinearFactorGraph.h>
Public Types | |
typedef FactorGraph< NonlinearFactor > | Base |
typedef std::shared_ptr< This > | shared_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< NonlinearFactor > | sharedFactor |
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 >::type & | operator+= (std::shared_ptr< DERIVEDFACTOR > factor) |
Append factor to factor graph. More... | |
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, This >::type & | operator, (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) |
This & | operator+= (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 |
sharedFactor & | at (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 |
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 |
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< HessianFactor > | linearizeToHessianFactor (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< 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. More... | |
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. 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< sharedFactor > | factors_ |
Definition at line 55 of file NonlinearFactorGraph.h.
Definition at line 59 of file NonlinearFactorGraph.h.
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.
typedef std::shared_ptr<This> gtsam::NonlinearFactorGraph::shared_ptr |
Definition at line 61 of file NonlinearFactorGraph.h.
Definition at line 60 of file NonlinearFactorGraph.h.
|
inline |
Default constructor
Definition at line 67 of file NonlinearFactorGraph.h.
|
inline |
Construct from iterator over factors
Definition at line 71 of file NonlinearFactorGraph.h.
|
inlineexplicit |
Construct from container of factors (shared_ptr or plain objects)
Definition at line 75 of file NonlinearFactorGraph.h.
|
inline |
Implicit copy/downcast constructor to override explicit template container constructor
Definition at line 79 of file NonlinearFactorGraph.h.
|
inline |
Directly add ExpressionFactor that implements |h(x)-z|^2_R
h | expression that implements measurement function |
z | measurement |
R | model |
Definition at line 187 of file NonlinearFactorGraph.h.
|
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).
Definition at line 215 of file NonlinearFactorGraph.h.
|
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 |
Definition at line 199 of file NonlinearFactorGraph.h.
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.
std::string gtsam::FactorGraph< FACTOR >::dot |
Output to graphviz format string.
Definition at line 168 of file FactorGraph-inst.h.
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.
void gtsam::FactorGraph< FACTOR >::dot |
Output to graphviz format, stream version.
Definition at line 141 of file FactorGraph-inst.h.
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.
bool gtsam::NonlinearFactorGraph::equals | ( | const NonlinearFactorGraph & | other, |
double | tol = 1e-9 |
||
) | const |
Test equality
Definition at line 97 of file NonlinearFactorGraph.cpp.
double gtsam::NonlinearFactorGraph::error | ( | const Values & | values | ) | const |
unnormalized error, in the most common case
Definition at line 170 of file NonlinearFactorGraph.cpp.
GaussianFactorGraph::shared_ptr gtsam::NonlinearFactorGraph::linearize | ( | const Values & | linearizationPoint | ) | const |
Linearize a nonlinear factor graph.
Definition at line 239 of file NonlinearFactorGraph.cpp.
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.
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.
|
private |
Linearize from Scatter rather than from Ordering. Made private because it doesn't include gttic.
Definition at line 312 of file NonlinearFactorGraph.cpp.
Ordering gtsam::NonlinearFactorGraph::orderingCOLAMD | ( | ) | const |
Compute a fill-reducing ordering using COLAMD.
Definition at line 182 of file NonlinearFactorGraph.cpp.
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 |
Definition at line 188 of file NonlinearFactorGraph.cpp.
|
overridevirtual |
Reimplemented from gtsam::FactorGraph< NonlinearFactor >.
Definition at line 55 of file NonlinearFactorGraph.cpp.
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.
double gtsam::NonlinearFactorGraph::probPrime | ( | const Values & | values | ) | const |
Unnormalized probability. O(n)
Definition at line 49 of file NonlinearFactorGraph.cpp.
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 |
Definition at line 384 of file NonlinearFactorGraph.cpp.
void gtsam::FactorGraph< FACTOR >::saveGraph |
output to file with graphviz format.
Definition at line 177 of file FactorGraph-inst.h.
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.
SymbolicFactorGraph::shared_ptr gtsam::NonlinearFactorGraph::symbolic | ( | ) | const |
Create a symbolic factor graph
Definition at line 194 of file NonlinearFactorGraph.cpp.
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.
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.