#include <IMUFactor.h>
Public Types | |
typedef NoiseModelFactorN< POSE, POSE > | Base |
typedef IMUFactor< POSE > | This |
Public Types inherited from gtsam::NoiseModelFactorN< POSE, POSE > | |
using | ValueType = typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type |
Public Types inherited from gtsam::NoiseModelFactor | |
typedef std::shared_ptr< This > | shared_ptr |
Public Types inherited from gtsam::NonlinearFactor | |
typedef std::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 Vector3 & | accel () const |
gtsam::NonlinearFactor::shared_ptr | clone () const override |
bool | equals (const NonlinearFactor &e, double tol=1e-9) const override |
virtual Vector | evaluateError (const Pose3 &x1, const Pose3 &x2, OptionalMatrixType H1, OptionalMatrixType H2) const |
Vector | evaluateError (const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override |
const Vector3 & | gyro () const |
IMUFactor (const Vector3 &accel, const Vector3 &gyro, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model) | |
IMUFactor (const Vector6 &imu_vector, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model) | |
void | print (const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override |
Vector6 | z () const |
~IMUFactor () override | |
Public Member Functions inherited from gtsam::NoiseModelFactorN< POSE, POSE > | |
Key | key () const |
virtual Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const =0 |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
~NoiseModelFactorN () override | |
NoiseModelFactorN () | |
Default Constructor for I/O. More... | |
NoiseModelFactorN (const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys) | |
NoiseModelFactorN (const SharedNoiseModel &noiseModel, CONTAINER keys) | |
Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const override |
virtual Vector | evaluateError (const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0 |
Vector | evaluateError (const ValueTypes &... x, MatrixTypeT< ValueTypes > &... H) const |
Vector | evaluateError (const ValueTypes &... x) const |
AreAllMatrixRefs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
AreAllMatrixPtrs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
Key | key1 () const |
Key | key2 () const |
Key | key3 () const |
Key | key4 () const |
Key | key5 () const |
Key | key6 () const |
Public Member Functions inherited from gtsam::NoiseModelFactor | |
shared_ptr | cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const |
size_t | dim () const override |
double | error (const Values &c) const override |
std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
const SharedNoiseModel & | noiseModel () const |
access to the noise model More... | |
NoiseModelFactor () | |
template<typename CONTAINER > | |
NoiseModelFactor (const SharedNoiseModel &noiseModel, const CONTAINER &keys) | |
Vector | unweightedWhitenedError (const Values &c) const |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
double | weight (const Values &c) const |
Vector | whitenedError (const Values &c) const |
~NoiseModelFactor () override | |
Public Member Functions inherited from gtsam::NonlinearFactor | |
NonlinearFactor () | |
template<typename CONTAINER > | |
NonlinearFactor (const CONTAINER &keys) | |
double | error (const HybridValues &c) const override |
virtual bool | active (const Values &) const |
virtual shared_ptr | rekey (const std::map< Key, Key > &rekey_mapping) const |
virtual shared_ptr | rekey (const KeyVector &new_keys) const |
virtual bool | sendable () const |
Public Member Functions inherited from gtsam::Factor | |
virtual | ~Factor ()=default |
Default destructor. More... | |
bool | empty () const |
Whether the factor is empty (involves zero variables). 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... | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Protected Attributes | |
Vector3 | accel_ |
double | dt_ |
Vector3 | gyro_ |
Protected Attributes inherited from gtsam::NoiseModelFactor | |
SharedNoiseModel | noiseModel_ |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
Static Private Member Functions | |
static Vector6 | predict_proxy (const PoseRTV &x1, const PoseRTV &x2, double dt, const Vector6 &meas) |
Additional Inherited Members | |
Static Public Attributes inherited from gtsam::NoiseModelFactorN< POSE, POSE > | |
constexpr static auto | N |
N is the number of variables (N-way factor) More... | |
Protected Types inherited from gtsam::NoiseModelFactorN< POSE, POSE > | |
using | Base = NoiseModelFactor |
using | KeyType = Key |
using | MatrixTypeT = Matrix |
using | OptionalMatrixTypeT = Matrix * |
using | This = NoiseModelFactorN< ValueTypes... > |
using | IsConvertible = typename std::enable_if< std::is_convertible< From, To >::value, void >::type |
using | IndexIsValid = typename std::enable_if<(I >=1) &&(I<=N), void >::type |
using | ContainerElementType = typename std::decay< decltype(*std::declval< Container >().begin())>::type |
using | IsContainerOfKeys = IsConvertible< ContainerElementType< Container >, Key > |
using | AreAllMatrixRefs = std::enable_if_t<(... &&std::is_convertible< Args, Matrix & >::value), Ret > |
using | IsMatrixPointer = std::is_same< typename std::decay_t< Arg >, Matrix * > |
using | IsNullpointer = std::is_same< typename std::decay_t< Arg >, std::nullptr_t > |
using | AreAllMatrixPtrs = std::enable_if_t<(... &&(IsMatrixPointer< Args >::value||IsNullpointer< Args >::value)), Ret > |
Protected Types inherited from gtsam::NoiseModelFactor | |
typedef NonlinearFactor | Base |
typedef NoiseModelFactor | This |
Protected Types inherited from gtsam::NonlinearFactor | |
typedef Factor | Base |
typedef NonlinearFactor | This |
Protected Member Functions inherited from gtsam::NoiseModelFactor | |
NoiseModelFactor (const SharedNoiseModel &noiseModel) | |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
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) |
Class that represents integrating IMU measurements over time for dynamic systems Templated to allow for different key types, but variables all assumed to be PoseRTV
Definition at line 23 of file IMUFactor.h.
typedef NoiseModelFactorN<POSE, POSE> gtsam::IMUFactor< POSE >::Base |
Definition at line 25 of file IMUFactor.h.
typedef IMUFactor<POSE> gtsam::IMUFactor< POSE >::This |
Definition at line 26 of file IMUFactor.h.
|
inline |
Standard constructor
Definition at line 40 of file IMUFactor.h.
|
inline |
Full IMU vector specification
Definition at line 45 of file IMUFactor.h.
|
inlineoverride |
Definition at line 49 of file IMUFactor.h.
|
inline |
Definition at line 75 of file IMUFactor.h.
|
inlineoverridevirtual |
Reimplemented from gtsam::NonlinearFactor.
Definition at line 52 of file IMUFactor.h.
|
inlineoverridevirtual |
Check if two factors are equal
Reimplemented from gtsam::NoiseModelFactor.
Definition at line 57 of file IMUFactor.h.
|
inlinevirtual |
dummy version that fails for non-dynamic poses
Definition at line 93 of file IMUFactor.h.
|
inlineoverride |
Error evaluation with optional derivatives - calculates z - h(x1,x2)
Definition at line 82 of file IMUFactor.h.
|
inline |
Definition at line 74 of file IMUFactor.h.
|
inlinestaticprivate |
copy of the measurement function formulated for numerical derivatives
Definition at line 101 of file IMUFactor.h.
|
inlineoverridevirtual |
|
inline |
Definition at line 76 of file IMUFactor.h.
|
protected |
measurements from the IMU
Definition at line 31 of file IMUFactor.h.
|
protected |
Definition at line 32 of file IMUFactor.h.
|
protected |
Definition at line 31 of file IMUFactor.h.