#include <EquivInertialNavFactor_GlobalVel.h>

Inheritance diagram for gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >:

[legend]

Public Types
typedef std::shared_ptr< EquivInertialNavFactor_GlobalVel >	shared_ptr

Public Types inherited from gtsam::NoiseModelFactorN< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >
enum
	N is the number of variables (N-way factor) More...

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
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, std::optional< IMUBIAS > Bias_initial={}, std::optional< POSE > body_P_sensor={})

Vector	evaluateError (const POSE &Pose1, const VELOCITY &Vel1, const IMUBIAS &Bias1, const POSE &Pose2, const VELOCITY &Vel2, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5) 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::NoiseModelFactorN< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >
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	clone () 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 ()

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 std::optional< IMUBIAS > &Bias_initial={})

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 std::optional< IMUBIAS > &Bias_initial={})

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 std::optional< IMUBIAS > &Bias_initial={})

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(), std::optional< POSE > p_body_P_sensor={})

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 NoiseModelFactorN< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >	Base

typedef EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >	This

Private Attributes
std::optional< IMUBIAS >	Bias_initial_

std::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_

Additional Inherited Members
Protected Types inherited from gtsam::NoiseModelFactorN< POSE, VELOCITY, IMUBIAS, POSE, VELOCITY >
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)

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 88 of file EquivInertialNavFactor_GlobalVel.h.

Member Typedef Documentation

◆ Base

template<class POSE , class VELOCITY , class IMUBIAS >

typedef NoiseModelFactorN<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Base

private

Definition at line 93 of file EquivInertialNavFactor_GlobalVel.h.

◆ shared_ptr

template<class POSE , class VELOCITY , class IMUBIAS >

typedef std::shared_ptr<EquivInertialNavFactor_GlobalVel> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::shared_ptr

Definition at line 115 of file EquivInertialNavFactor_GlobalVel.h.

◆ This

template<class POSE , class VELOCITY , class IMUBIAS >

typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::This

private

Definition at line 92 of file EquivInertialNavFactor_GlobalVel.h.

Constructor & Destructor Documentation

◆ EquivInertialNavFactor_GlobalVel() [1/2]

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 118 of file EquivInertialNavFactor_GlobalVel.h.

◆ EquivInertialNavFactor_GlobalVel() [2/2]

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,
		std::optional< IMUBIAS >	Bias_initial = `{}`,
		std::optional< POSE >	body_P_sensor = `{}`
	)

inline

Constructor

Definition at line 121 of file EquivInertialNavFactor_GlobalVel.h.

◆ ~EquivInertialNavFactor_GlobalVel()

template<class POSE , class VELOCITY , class IMUBIAS >

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

inlineoverride

Definition at line 132 of file EquivInertialNavFactor_GlobalVel.h.

Member Function Documentation

◆ calc_descrete_noise_model()

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 698 of file EquivInertialNavFactor_GlobalVel.h.

◆ Calc_g_rho_omega_earth_ENU()

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 643 of file EquivInertialNavFactor_GlobalVel.h.

◆ Calc_g_rho_omega_earth_NED()

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 613 of file EquivInertialNavFactor_GlobalVel.h.

◆ CalcEquivalentNoiseCov()

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 591 of file EquivInertialNavFactor_GlobalVel.h.

◆ CalcEquivalentNoiseCov_DifferentParts()

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 604 of file EquivInertialNavFactor_GlobalVel.h.

◆ equals()

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 157 of file EquivInertialNavFactor_GlobalVel.h.

◆ evaluateError()

template<class POSE , class VELOCITY , class IMUBIAS >

Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::evaluateError	(	const POSE &	Pose1,
		const VELOCITY &	Vel1,
		const IMUBIAS &	Bias1,
		const POSE &	Pose2,
		const VELOCITY &	Vel2,
		OptionalMatrixType	H1,
		OptionalMatrixType	H2,
		OptionalMatrixType	H3,
		OptionalMatrixType	H4,
		OptionalMatrixType	H5
	)		const

inlineoverride

Definition at line 301 of file EquivInertialNavFactor_GlobalVel.h.

◆ evaluatePoseError()

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 281 of file EquivInertialNavFactor_GlobalVel.h.

◆ evaluateVelocityError()

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 293 of file EquivInertialNavFactor_GlobalVel.h.

◆ predict()

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 276 of file EquivInertialNavFactor_GlobalVel.h.

◆ PredictFromPreIntegration()

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 std::optional< IMUBIAS > &	Bias_initial = `{}`
	)

inlinestatic

Definition at line 432 of file EquivInertialNavFactor_GlobalVel.h.

◆ predictPose()

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 171 of file EquivInertialNavFactor_GlobalVel.h.

◆ predictPose_inertial()

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 198 of file EquivInertialNavFactor_GlobalVel.h.

◆ PredictPoseFromPreIntegration()

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 std::optional< IMUBIAS > &	Bias_initial = `{}`
	)

inlinestatic

Definition at line 381 of file EquivInertialNavFactor_GlobalVel.h.

◆ predictVelocity()

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 236 of file EquivInertialNavFactor_GlobalVel.h.

◆ predictVelocity_inertial()

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 256 of file EquivInertialNavFactor_GlobalVel.h.

◆ PredictVelocityFromPreIntegration()

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 std::optional< IMUBIAS > &	Bias_initial = `{}`
	)

inlinestatic

Definition at line 411 of file EquivInertialNavFactor_GlobalVel.h.

◆ PreIntegrateIMUObservations()

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()`,
		std::optional< POSE >	p_body_P_sensor = `{}`
	)

inlinestatic

Definition at line 443 of file EquivInertialNavFactor_GlobalVel.h.

◆ PreIntegrateIMUObservations_delta_angles()

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 568 of file EquivInertialNavFactor_GlobalVel.h.

◆ PreIntegrateIMUObservations_delta_pos()

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 529 of file EquivInertialNavFactor_GlobalVel.h.

◆ PreIntegrateIMUObservations_delta_vel()

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 540 of file EquivInertialNavFactor_GlobalVel.h.

◆ print()

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 137 of file EquivInertialNavFactor_GlobalVel.h.

Member Data Documentation

◆ Bias_initial_

template<class POSE , class VELOCITY , class IMUBIAS >

std::optional<IMUBIAS> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Bias_initial_

private

Definition at line 106 of file EquivInertialNavFactor_GlobalVel.h.

◆ body_P_sensor_

template<class POSE , class VELOCITY , class IMUBIAS >

std::optional<POSE> gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::body_P_sensor_

private

Definition at line 107 of file EquivInertialNavFactor_GlobalVel.h.

◆ delta_angles_

template<class POSE , class VELOCITY , class IMUBIAS >

Vector3 gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::delta_angles_

private

Definition at line 97 of file EquivInertialNavFactor_GlobalVel.h.

◆ delta_pos_in_t0_

template<class POSE , class VELOCITY , class IMUBIAS >

Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::delta_pos_in_t0_

private

Definition at line 95 of file EquivInertialNavFactor_GlobalVel.h.

◆ delta_vel_in_t0_

template<class POSE , class VELOCITY , class IMUBIAS >

Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::delta_vel_in_t0_

private

Definition at line 96 of file EquivInertialNavFactor_GlobalVel.h.

◆ dt12_

template<class POSE , class VELOCITY , class IMUBIAS >

double gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::dt12_

private

Definition at line 98 of file EquivInertialNavFactor_GlobalVel.h.

◆ Jacobian_wrt_t0_Overall_

template<class POSE , class VELOCITY , class IMUBIAS >

Matrix gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::Jacobian_wrt_t0_Overall_

private

Definition at line 104 of file EquivInertialNavFactor_GlobalVel.h.

◆ world_g_

template<class POSE , class VELOCITY , class IMUBIAS >

Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::world_g_

private

Definition at line 100 of file EquivInertialNavFactor_GlobalVel.h.

◆ world_omega_earth_

template<class POSE , class VELOCITY , class IMUBIAS >

Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::world_omega_earth_

private

Definition at line 102 of file EquivInertialNavFactor_GlobalVel.h.

◆ world_rho_

template<class POSE , class VELOCITY , class IMUBIAS >

Vector gtsam::EquivInertialNavFactor_GlobalVel< POSE, VELOCITY, IMUBIAS >::world_rho_

private

Definition at line 101 of file EquivInertialNavFactor_GlobalVel.h.

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

EquivInertialNavFactor_GlobalVel.h

Public Types

Public Member Functions

Static Public Member Functions

Private Types

Private Attributes

Additional Inherited Members

Detailed Description

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

Member Typedef Documentation

◆ Base

◆ shared_ptr

◆ This

Constructor & Destructor Documentation

◆ EquivInertialNavFactor_GlobalVel() [1/2]

◆ EquivInertialNavFactor_GlobalVel() [2/2]

◆ ~EquivInertialNavFactor_GlobalVel()

Member Function Documentation

◆ calc_descrete_noise_model()

◆ Calc_g_rho_omega_earth_ENU()

◆ Calc_g_rho_omega_earth_NED()

◆ CalcEquivalentNoiseCov()

◆ CalcEquivalentNoiseCov_DifferentParts()

◆ equals()

◆ evaluateError()

◆ evaluatePoseError()

◆ evaluateVelocityError()

◆ predict()

◆ PredictFromPreIntegration()

◆ predictPose()

◆ predictPose_inertial()

◆ PredictPoseFromPreIntegration()

◆ predictVelocity()

◆ predictVelocity_inertial()

◆ PredictVelocityFromPreIntegration()

◆ PreIntegrateIMUObservations()

◆ PreIntegrateIMUObservations_delta_angles()

◆ PreIntegrateIMUObservations_delta_pos()

◆ PreIntegrateIMUObservations_delta_vel()

◆ print()

Member Data Documentation

◆ Bias_initial_

◆ body_P_sensor_

◆ delta_angles_

◆ delta_pos_in_t0_

◆ delta_vel_in_t0_

◆ dt12_

◆ Jacobian_wrt_t0_Overall_

◆ world_g_

◆ world_omega_earth_

◆ world_rho_

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