Container of NonlinearEqualityConstraint. More...
#include <NonlinearEqualityConstraint.h>
Public Types | |
typedef FactorGraph< NonlinearEqualityConstraint > | Base |
typedef std::shared_ptr< NonlinearEqualityConstraints > | shared_ptr |
![]() | |
typedef FastVector< sharedFactor >::const_iterator | const_iterator |
typedef NonlinearEqualityConstraint | FactorType |
factor type More... | |
typedef FastVector< sharedFactor >::iterator | iterator |
typedef std::shared_ptr< NonlinearEqualityConstraint > | sharedFactor |
Shared pointer to a factor. More... | |
typedef sharedFactor | value_type |
Public Member Functions | |
size_t | dim () const |
NonlinearFactorGraph | penaltyGraph (const double mu=1.0) const |
double | violationNorm (const Values &values) const |
Evaluate the constraint violation (as 2-norm of the violation vector). More... | |
Vector | violationVector (const Values &values, bool whiten=true) const |
Evaluate the constraint violation as a vector. More... | |
![]() | |
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) |
virtual void | print (const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const |
Print out graph to std::cout, with optional key formatter. More... | |
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 |
Static Public Member Functions | |
static NonlinearEqualityConstraints | FromCostGraph (const NonlinearFactorGraph &graph) |
Create constraints ensuring the cost of factors of a graph is zero. More... | |
Additional Inherited Members | |
![]() | |
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) | |
![]() | |
FastVector< sharedFactor > | factors_ |
Container of NonlinearEqualityConstraint.
Definition at line 147 of file NonlinearEqualityConstraint.h.
Definition at line 150 of file NonlinearEqualityConstraint.h.
typedef std::shared_ptr<NonlinearEqualityConstraints> gtsam::NonlinearEqualityConstraints::shared_ptr |
Definition at line 149 of file NonlinearEqualityConstraint.h.
size_t gtsam::NonlinearEqualityConstraints::dim | ( | ) | const |
Definition at line 49 of file NonlinearEqualityConstraint.cpp.
|
static |
Create constraints ensuring the cost of factors of a graph is zero.
Definition at line 38 of file NonlinearEqualityConstraint.cpp.
NonlinearFactorGraph gtsam::NonlinearEqualityConstraints::penaltyGraph | ( | const double | mu = 1.0 | ) | const |
Definition at line 76 of file NonlinearEqualityConstraint.cpp.
double gtsam::NonlinearEqualityConstraints::violationNorm | ( | const Values & | values | ) | const |
Evaluate the constraint violation (as 2-norm of the violation vector).
Definition at line 71 of file NonlinearEqualityConstraint.cpp.
Vector gtsam::NonlinearEqualityConstraints::violationVector | ( | const Values & | values, |
bool | whiten = true |
||
) | const |
Evaluate the constraint violation as a vector.
Definition at line 58 of file NonlinearEqualityConstraint.cpp.