Public Types | Public Member Functions | Static Public Member Functions | Private Types | Private Member Functions | Private Attributes | Friends | List of all members
gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS > Class Template Reference

#include <EquivInertialNavFactor_GlobalVel.h>

Inheritance diagram for gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >:
Inheritance graph
[legend]

Public Types

typedef boost::shared_ptr< EquivInertialNavFactor_GlobalVel > shared_ptr
 
- Public Types inherited from gtsam::NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >
typedef POSE X1
 
typedef VELOCITY X2
 
typedef IMUBIAS X3
 
typedef POSE X4
 
typedef VELOCITY X5
 
- Public Types inherited from gtsam::NoiseModelFactor
typedef boost::shared_ptr< This > shared_ptr
 
- 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

bool equals (const NonlinearFactor &expected, double tol=1e-9) const override
 
 EquivInertialNavFactor_GlobalVel ()
 
 EquivInertialNavFactor_GlobalVel (const Key &Pose1, const Key &Vel1, const Key &IMUBias1, const Key &Pose2, const Key &Vel2, const Vector &delta_pos_in_t0, const Vector &delta_vel_in_t0, const Vector3 &delta_angles, double dt12, const Vector world_g, const Vector world_rho, const Vector &world_omega_earth, const noiseModel::Gaussian::shared_ptr &model_equivalent, const Matrix &Jacobian_wrt_t0_Overall, boost::optional< IMUBIAS > Bias_initial=boost::none, boost::optional< POSE > body_P_sensor=boost::none)
 
Vector evaluateError (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none, boost::optional< Matrix & > H5=boost::none) const override
 
POSE evaluatePoseError (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2) const
 
VELOCITY evaluateVelocityError (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2) const
 
void predict (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, POSE &Pose2, VELOCITY &Vel2) const
 
POSE predictPose (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1) const
 
VELOCITY predictVelocity (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1) const
 
void print (const std::string &s="EquivInertialNavFactor_GlobalVel", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 
 ~EquivInertialNavFactor_GlobalVel () override
 
- Public Member Functions inherited from gtsam::NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >
Key key1 () const
 
Key key2 () const
 
Key key3 () const
 
Key key4 () const
 
Key key5 () const
 
 NoiseModelFactor5 ()
 
 NoiseModelFactor5 (const SharedNoiseModel &noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5)
 
Vector unwhitenedError (const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
 
 ~NoiseModelFactor5 () override
 
- 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
 
boost::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
 
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)
 
virtual ~NonlinearFactor ()
 
virtual bool active (const Values &) const
 
virtual shared_ptr clone () 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...
 
KeyVector & keys ()
 
iterator begin ()
 
iterator end ()
 
virtual void printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
 print only keys 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
 

Static Public Member Functions

static noiseModel::Gaussian::shared_ptr calc_descrete_noise_model (const noiseModel::Gaussian::shared_ptr &model, double delta_t)
 
static void Calc_g_rho_omega_earth_ENU (const Vector &Pos_ENU, const Vector &Vel_ENU, const Vector &LatLonHeight_IC, const Vector &Pos_ENU_Initial, Vector &g_ENU, Vector &rho_ENU, Vector &omega_earth_ENU)
 
static void Calc_g_rho_omega_earth_NED (const Vector &Pos_NED, const Vector &Vel_NED, const Vector &LatLonHeight_IC, const Vector &Pos_NED_Initial, Vector &g_NED, Vector &rho_NED, Vector &omega_earth_NED)
 
static noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov (const noiseModel::Gaussian::shared_ptr &gaussian_acc, const noiseModel::Gaussian::shared_ptr &gaussian_gyro, const noiseModel::Gaussian::shared_ptr &gaussian_process)
 
static void CalcEquivalentNoiseCov_DifferentParts (const noiseModel::Gaussian::shared_ptr &gaussian_acc, const noiseModel::Gaussian::shared_ptr &gaussian_gyro, const noiseModel::Gaussian::shared_ptr &gaussian_process, Matrix &cov_acc, Matrix &cov_gyro, Matrix &cov_process_without_acc_gyro)
 
static void PredictFromPreIntegration (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, POSE &Pose2, VELOCITY &Vel2, const Vector &delta_pos_in_t0, const Vector &delta_vel_in_t0, const Vector3 &delta_angles, double dt12, const Vector world_g, const Vector world_rho, const Vector &world_omega_earth, const Matrix &Jacobian_wrt_t0_Overall, const boost::optional< IMUBIAS > &Bias_initial=boost::none)
 
static POSE predictPose_inertial (const POSE &Pose1, const VELOCITY &Vel1, const Vector &delta_pos_in_t0, const Vector3 &delta_angles, const double dt12, const Vector &world_g, const Vector &world_rho, const Vector &world_omega_earth)
 
static POSE PredictPoseFromPreIntegration (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const Vector &delta_pos_in_t0, const Vector3 &delta_angles, double dt12, const Vector world_g, const Vector world_rho, const Vector &world_omega_earth, const Matrix &Jacobian_wrt_t0_Overall, const boost::optional< IMUBIAS > &Bias_initial=boost::none)
 
static VELOCITY predictVelocity_inertial (const POSE &Pose1, const VELOCITY &Vel1, const Vector &delta_vel_in_t0, const double dt12, const Vector &world_g, const Vector &world_rho, const Vector &world_omega_earth)
 
static VELOCITY PredictVelocityFromPreIntegration (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const Vector &delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho, const Vector &world_omega_earth, const Matrix &Jacobian_wrt_t0_Overall, const boost::optional< IMUBIAS > &Bias_initial=boost::none)
 
static void PreIntegrateIMUObservations (const Vector &msr_acc_t, const Vector &msr_gyro_t, const double msr_dt, Vector &delta_pos_in_t0, Vector3 &delta_angles, Vector &delta_vel_in_t0, double &delta_t, const noiseModel::Gaussian::shared_ptr &model_continuous_overall, Matrix &EquivCov_Overall, Matrix &Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0=IMUBIAS(), boost::optional< POSE > p_body_P_sensor=boost::none)
 
static Vector PreIntegrateIMUObservations_delta_angles (const Vector &msr_gyro_t, const double msr_dt, const Vector3 &delta_angles, const bool flag_use_body_P_sensor, const POSE &body_P_sensor, IMUBIAS Bias_t0=IMUBIAS())
 
static Vector PreIntegrateIMUObservations_delta_pos (const double msr_dt, const Vector &delta_pos_in_t0, const Vector &delta_vel_in_t0)
 
static Vector PreIntegrateIMUObservations_delta_vel (const Vector &msr_gyro_t, const Vector &msr_acc_t, const double msr_dt, const Vector3 &delta_angles, const Vector &delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE &body_P_sensor, IMUBIAS Bias_t0=IMUBIAS())
 

Private Types

typedef NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY > Base
 
typedef EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS > This
 

Private Member Functions

template<class ARCHIVE >
void serialize (ARCHIVE &ar, const unsigned int)
 

Private Attributes

boost::optional< IMUBIAS > Bias_initial_
 
boost::optional< POSE > body_P_sensor_
 
Vector3 delta_angles_
 
Vector delta_pos_in_t0_
 
Vector delta_vel_in_t0_
 
double dt12_
 
Matrix Jacobian_wrt_t0_Overall_
 
Vector world_g_
 
Vector world_omega_earth_
 
Vector world_rho_
 

Friends

class boost::serialization::access
 

Additional Inherited Members

- Protected Types inherited from gtsam::NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >
typedef NoiseModelFactor Base
 
typedef NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY > This
 
- 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)
 
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)
 
- Protected Attributes inherited from gtsam::NoiseModelFactor
SharedNoiseModel noiseModel_
 
- Protected Attributes inherited from gtsam::Factor
KeyVector keys_
 The keys involved in this factor. More...
 

Detailed Description

template<class POSE, class VELOCITY, class IMUBIAS>
class gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >

Definition at line 90 of file EquivInertialNavFactor_GlobalVel.h.

Member Typedef Documentation

template<class POSE , class VELOCITY , class IMUBIAS >
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Base
private

Definition at line 95 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
typedef boost::shared_ptr<EquivInertialNavFactor_GlobalVel> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::shared_ptr

Definition at line 114 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::This
private

Definition at line 94 of file EquivInertialNavFactor_GlobalVel.h.

Constructor & Destructor Documentation

template<class POSE , class VELOCITY , class IMUBIAS >
gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::EquivInertialNavFactor_GlobalVel ( )
inline

default constructor - only use for serialization

Definition at line 117 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::EquivInertialNavFactor_GlobalVel ( const Key &  Pose1,
const Key &  Vel1,
const Key &  IMUBias1,
const Key &  Pose2,
const Key &  Vel2,
const Vector &  delta_pos_in_t0,
const Vector &  delta_vel_in_t0,
const Vector3 &  delta_angles,
double  dt12,
const Vector  world_g,
const Vector  world_rho,
const Vector &  world_omega_earth,
const noiseModel::Gaussian::shared_ptr &  model_equivalent,
const Matrix &  Jacobian_wrt_t0_Overall,
boost::optional< IMUBIAS >  Bias_initial = boost::none,
boost::optional< POSE >  body_P_sensor = boost::none 
)
inline

Constructor

Definition at line 120 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::~EquivInertialNavFactor_GlobalVel ( )
inlineoverride

Definition at line 131 of file EquivInertialNavFactor_GlobalVel.h.

Member Function Documentation

template<class POSE , class VELOCITY , class IMUBIAS >
static noiseModel::Gaussian::shared_ptr gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::calc_descrete_noise_model ( const noiseModel::Gaussian::shared_ptr &  model,
double  delta_t 
)
inlinestatic

Definition at line 643 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Calc_g_rho_omega_earth_ENU ( const Vector &  Pos_ENU,
const Vector &  Vel_ENU,
const Vector &  LatLonHeight_IC,
const Vector &  Pos_ENU_Initial,
Vector &  g_ENU,
Vector &  rho_ENU,
Vector &  omega_earth_ENU 
)
inlinestatic

Definition at line 588 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Calc_g_rho_omega_earth_NED ( const Vector &  Pos_NED,
const Vector &  Vel_NED,
const Vector &  LatLonHeight_IC,
const Vector &  Pos_NED_Initial,
Vector &  g_NED,
Vector &  rho_NED,
Vector &  omega_earth_NED 
)
inlinestatic

Definition at line 558 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static noiseModel::Gaussian::shared_ptr gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::CalcEquivalentNoiseCov ( const noiseModel::Gaussian::shared_ptr &  gaussian_acc,
const noiseModel::Gaussian::shared_ptr &  gaussian_gyro,
const noiseModel::Gaussian::shared_ptr &  gaussian_process 
)
inlinestatic

Definition at line 536 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::CalcEquivalentNoiseCov_DifferentParts ( const noiseModel::Gaussian::shared_ptr &  gaussian_acc,
const noiseModel::Gaussian::shared_ptr &  gaussian_gyro,
const noiseModel::Gaussian::shared_ptr &  gaussian_process,
Matrix &  cov_acc,
Matrix &  cov_gyro,
Matrix &  cov_process_without_acc_gyro 
)
inlinestatic

Definition at line 549 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
bool gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::equals ( const NonlinearFactor &  expected,
double  tol = 1e-9 
) const
inlineoverridevirtual

equals

Reimplemented from gtsam::NoiseModelFactor.

Definition at line 156 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::evaluateError ( const POSE &  ,
const VELOCITY &  ,
const IMUBIAS &  ,
const POSE &  ,
const VELOCITY &  ,
boost::optional< Matrix & >  H1 = boost::none,
boost::optional< Matrix & >  H2 = boost::none,
boost::optional< Matrix & >  H3 = boost::none,
boost::optional< Matrix & >  H4 = boost::none,
boost::optional< Matrix & >  H5 = boost::none 
) const
inlineoverridevirtual

Override this method to finish implementing a 5-way factor. If any of the optional Matrix reference arguments are specified, it should compute both the function evaluation and its derivative(s) in X1 (and/or X2, X3).

Implements gtsam::NoiseModelFactor5< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >.

Definition at line 300 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
POSE gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::evaluatePoseError ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1,
const POSE &  Pose2,
const VELOCITY &  Vel2 
) const
inline

Definition at line 280 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
VELOCITY gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::evaluateVelocityError ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1,
const POSE &  Pose2,
const VELOCITY &  Vel2 
) const
inline

Definition at line 292 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::predict ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1,
POSE &  Pose2,
VELOCITY &  Vel2 
) const
inline

Definition at line 275 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::PredictFromPreIntegration ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1,
POSE &  Pose2,
VELOCITY &  Vel2,
const Vector &  delta_pos_in_t0,
const Vector &  delta_vel_in_t0,
const Vector3 &  delta_angles,
double  dt12,
const Vector  world_g,
const Vector  world_rho,
const Vector &  world_omega_earth,
const Matrix &  Jacobian_wrt_t0_Overall,
const boost::optional< IMUBIAS > &  Bias_initial = boost::none 
)
inlinestatic

Definition at line 404 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
POSE gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::predictPose ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1 
) const
inline

Definition at line 170 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static POSE gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::predictPose_inertial ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const Vector &  delta_pos_in_t0,
const Vector3 &  delta_angles,
const double  dt12,
const Vector &  world_g,
const Vector &  world_rho,
const Vector &  world_omega_earth 
)
inlinestatic

Definition at line 197 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static POSE gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::PredictPoseFromPreIntegration ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1,
const Vector &  delta_pos_in_t0,
const Vector3 &  delta_angles,
double  dt12,
const Vector  world_g,
const Vector  world_rho,
const Vector &  world_omega_earth,
const Matrix &  Jacobian_wrt_t0_Overall,
const boost::optional< IMUBIAS > &  Bias_initial = boost::none 
)
inlinestatic

Definition at line 353 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
VELOCITY gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::predictVelocity ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1 
) const
inline

Definition at line 235 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static VELOCITY gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::predictVelocity_inertial ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const Vector &  delta_vel_in_t0,
const double  dt12,
const Vector &  world_g,
const Vector &  world_rho,
const Vector &  world_omega_earth 
)
inlinestatic

Definition at line 255 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static VELOCITY gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::PredictVelocityFromPreIntegration ( const POSE &  Pose1,
const VELOCITY &  Vel1,
const IMUBIAS &  Bias1,
const Vector &  delta_vel_in_t0,
double  dt12,
const Vector  world_g,
const Vector  world_rho,
const Vector &  world_omega_earth,
const Matrix &  Jacobian_wrt_t0_Overall,
const boost::optional< IMUBIAS > &  Bias_initial = boost::none 
)
inlinestatic

Definition at line 383 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::PreIntegrateIMUObservations ( const Vector &  msr_acc_t,
const Vector &  msr_gyro_t,
const double  msr_dt,
Vector &  delta_pos_in_t0,
Vector3 &  delta_angles,
Vector &  delta_vel_in_t0,
double &  delta_t,
const noiseModel::Gaussian::shared_ptr &  model_continuous_overall,
Matrix &  EquivCov_Overall,
Matrix &  Jacobian_wrt_t0_Overall,
const IMUBIAS  Bias_t0 = IMUBIAS(),
boost::optional< POSE >  p_body_P_sensor = boost::none 
)
inlinestatic

Definition at line 415 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::PreIntegrateIMUObservations_delta_angles ( const Vector &  msr_gyro_t,
const double  msr_dt,
const Vector3 &  delta_angles,
const bool  flag_use_body_P_sensor,
const POSE &  body_P_sensor,
IMUBIAS  Bias_t0 = IMUBIAS() 
)
inlinestatic

Definition at line 513 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::PreIntegrateIMUObservations_delta_pos ( const double  msr_dt,
const Vector &  delta_pos_in_t0,
const Vector &  delta_vel_in_t0 
)
inlinestatic

Definition at line 474 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
static Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::PreIntegrateIMUObservations_delta_vel ( const Vector &  msr_gyro_t,
const Vector &  msr_acc_t,
const double  msr_dt,
const Vector3 &  delta_angles,
const Vector &  delta_vel_in_t0,
const bool  flag_use_body_P_sensor,
const POSE &  body_P_sensor,
IMUBIAS  Bias_t0 = IMUBIAS() 
)
inlinestatic

Definition at line 485 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::print ( const std::string &  s = "EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >",
const KeyFormatter &  keyFormatter = DefaultKeyFormatter 
) const
inlineoverridevirtual

implement functions needed for Testable print

Reimplemented from gtsam::NoiseModelFactor.

Definition at line 136 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
template<class ARCHIVE >
void gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::serialize ( ARCHIVE &  ar,
const unsigned  int 
)
inlineprivate

Definition at line 655 of file EquivInertialNavFactor_GlobalVel.h.

Friends And Related Function Documentation

template<class POSE , class VELOCITY , class IMUBIAS >
friend class boost::serialization::access
friend

Serialization function

Definition at line 653 of file EquivInertialNavFactor_GlobalVel.h.

Member Data Documentation

template<class POSE , class VELOCITY , class IMUBIAS >
boost::optional<IMUBIAS> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Bias_initial_
private

Definition at line 108 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
boost::optional<POSE> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::body_P_sensor_
private

Definition at line 109 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Vector3 gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::delta_angles_
private

Definition at line 99 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::delta_pos_in_t0_
private

Definition at line 97 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::delta_vel_in_t0_
private

Definition at line 98 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
double gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::dt12_
private

Definition at line 100 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Matrix gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Jacobian_wrt_t0_Overall_
private

Definition at line 106 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::world_g_
private

Definition at line 102 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::world_omega_earth_
private

Definition at line 104 of file EquivInertialNavFactor_GlobalVel.h.

template<class POSE , class VELOCITY , class IMUBIAS >
Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::world_rho_
private

Definition at line 103 of file EquivInertialNavFactor_GlobalVel.h.

The documentation for this class was generated from the following file:

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:08