#include <LinearizedFactor.h>
Public Types | |
typedef VerticalBlockMatrix::Block | ABlock |
typedef LinearizedGaussianFactor | Base |
typedef VerticalBlockMatrix::Block::ColXpr | BVector |
typedef VerticalBlockMatrix::constBlock | constABlock |
typedef VerticalBlockMatrix::constBlock::ConstColXpr | constBVector |
typedef boost::shared_ptr< LinearizedJacobianFactor > | shared_ptr |
typedef LinearizedJacobianFactor | This |
Public Types inherited from gtsam::LinearizedGaussianFactor | |
typedef NonlinearFactor | Base |
typedef boost::shared_ptr< LinearizedGaussianFactor > | shared_ptr |
typedef LinearizedGaussianFactor | This |
Public Types inherited from gtsam::NonlinearFactor | |
typedef boost::shared_ptr< This > | shared_ptr |
Public Types inherited from gtsam::Factor | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. More... | |
typedef KeyVector::iterator | iterator |
Iterator over keys. More... | |
Public Member Functions | |
const constABlock | A () const |
const constABlock | A (Key key) const |
const constBVector | b () const |
gtsam::NonlinearFactor::shared_ptr | clone () const override |
size_t | dim () const override |
bool | equals (const NonlinearFactor &expected, double tol=1e-9) const override |
double | error (const Values &c) const override |
Vector | error_vector (const Values &c) const |
boost::shared_ptr< GaussianFactor > | linearize (const Values &c) const override |
LinearizedJacobianFactor () | |
LinearizedJacobianFactor (const JacobianFactor::shared_ptr &jacobian, const Values &lin_points) | |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
~LinearizedJacobianFactor () override | |
Public Member Functions inherited from gtsam::LinearizedGaussianFactor | |
const Values & | linearizationPoint () const |
LinearizedGaussianFactor () | |
LinearizedGaussianFactor (const GaussianFactor::shared_ptr &gaussian, const Values &lin_points) | |
~LinearizedGaussianFactor () override | |
Public Member Functions inherited from gtsam::NonlinearFactor | |
NonlinearFactor () | |
template<typename CONTAINER > | |
NonlinearFactor (const CONTAINER &keys) | |
virtual | ~NonlinearFactor () |
virtual bool | active (const Values &) const |
shared_ptr | rekey (const std::map< Key, Key > &rekey_mapping) const |
shared_ptr | rekey (const KeyVector &new_keys) const |
Public Member Functions inherited from gtsam::Factor | |
virtual | ~Factor ()=default |
Default destructor. More... | |
Key | front () const |
First key. More... | |
Key | back () const |
Last key. More... | |
const_iterator | find (Key key) const |
find More... | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. More... | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Protected Attributes | |
VerticalBlockMatrix | Ab_ |
Protected Attributes inherited from gtsam::LinearizedGaussianFactor | |
Values | lin_points_ |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
Private Member Functions | |
template<class ARCHIVE > | |
void | serialize (ARCHIVE &ar, const unsigned int) |
Friends | |
class | boost::serialization::access |
Additional Inherited Members | |
Protected Types inherited from gtsam::NonlinearFactor | |
typedef Factor | Base |
typedef NonlinearFactor | This |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
Static Protected Member Functions inherited from gtsam::Factor | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
A factor that takes a linear, Jacobian factor and inserts it into a nonlinear graph.
Definition at line 77 of file LinearizedFactor.h.
Definition at line 87 of file LinearizedFactor.h.
base type
Definition at line 81 of file LinearizedFactor.h.
Definition at line 89 of file LinearizedFactor.h.
Definition at line 88 of file LinearizedFactor.h.
Definition at line 90 of file LinearizedFactor.h.
typedef boost::shared_ptr<LinearizedJacobianFactor> gtsam::LinearizedJacobianFactor::shared_ptr |
shared pointer for convenience
Definition at line 85 of file LinearizedFactor.h.
Definition at line 82 of file LinearizedFactor.h.
gtsam::LinearizedJacobianFactor::LinearizedJacobianFactor | ( | ) |
default constructor for serialization
Definition at line 39 of file LinearizedFactor.cpp.
gtsam::LinearizedJacobianFactor::LinearizedJacobianFactor | ( | const JacobianFactor::shared_ptr & | jacobian, |
const Values & | lin_points | ||
) |
jacobian | A jacobian factor |
lin_points | The linearization points for, at least, the variables used by this factor |
Definition at line 43 of file LinearizedFactor.cpp.
|
inlineoverride |
Definition at line 112 of file LinearizedFactor.h.
|
inline |
Definition at line 129 of file LinearizedFactor.h.
|
inline |
Definition at line 130 of file LinearizedFactor.h.
|
inline |
Definition at line 128 of file LinearizedFactor.h.
|
inlineoverridevirtual |
Reimplemented from gtsam::NonlinearFactor.
Definition at line 115 of file LinearizedFactor.h.
|
inlineoverridevirtual |
get the dimension of the factor (number of rows on linearization)
Implements gtsam::NonlinearFactor.
Definition at line 133 of file LinearizedFactor.h.
|
overridevirtual |
equals function with optional tolerance
Reimplemented from gtsam::NonlinearFactor.
Definition at line 79 of file LinearizedFactor.cpp.
|
overridevirtual |
Calculate the error of the factor
Implements gtsam::NonlinearFactor.
Definition at line 96 of file LinearizedFactor.cpp.
(A*x-b)
Definition at line 118 of file LinearizedFactor.cpp.
|
overridevirtual |
linearize to a GaussianFactor Reimplemented from NoiseModelFactor to directly copy out the matrices and only update the RHS b with an updated residual
Implements gtsam::NonlinearFactor.
Definition at line 103 of file LinearizedFactor.cpp.
|
overridevirtual |
print function
Reimplemented from gtsam::NonlinearFactor.
Definition at line 62 of file LinearizedFactor.cpp.
|
inlineprivate |
Definition at line 152 of file LinearizedFactor.h.
|
friend |
Serialization function
Definition at line 150 of file LinearizedFactor.h.
|
protected |
Definition at line 99 of file LinearizedFactor.h.