#include <NavState.h>

Inheritance diagram for gtsam::NavState:

Classes
struct	ChartAtOrigin

Public Member Functions
Component Access
const Rot3 &	attitude (OptionalJacobian< 3, 9 > H={}) const

const Point3 &	position (OptionalJacobian< 3, 9 > H={}) const

const Velocity3 &	velocity (OptionalJacobian< 3, 9 > H={}) const

const Pose3	pose () const

Derived quantities
Matrix3	R () const
	Return rotation matrix. Induces computation in quaternion mode. More...

Quaternion	quaternion () const
	Return quaternion. Induces computation in matrix mode. More...

Vector3	t () const
	Return position as Vector3. More...

const Vector3 &	v () const
	Return velocity as Vector3. Computation-free. More...

Velocity3	bodyVelocity (OptionalJacobian< 3, 9 > H={}) const

Matrix5	matrix () const

Dynamics
NavState	update (const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F={}, OptionalJacobian< 9, 3 > G1={}, OptionalJacobian< 9, 3 > G2={}) const

Vector9	coriolis (double dt, const Vector3 &omega, bool secondOrder=false, OptionalJacobian< 9, 9 > H={}) const
	Compute tangent space contribution due to Coriolis forces. More...

Vector9	correctPIM (const Vector9 &pim, double dt, const Vector3 &n_gravity, const std::optional< Vector3 > &omegaCoriolis, bool use2ndOrderCoriolis=false, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const

Public Member Functions inherited from gtsam::LieGroup< NavState, 9 >
NavState	between (const NavState &g) const

NavState	between (const NavState &g, ChartJacobian H1, ChartJacobian H2={}) const

SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

GTSAM_EXPORT SOn	between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

NavState	compose (const NavState &g) const

NavState	compose (const NavState &g, ChartJacobian H1, ChartJacobian H2={}) const

SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

GTSAM_EXPORT SOn	compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const

const NavState &	derived () const

NavState	expmap (const TangentVector &v) const

NavState	expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
	expmap with optional derivatives More...

NavState	inverse (ChartJacobian H) const

TangentVector	localCoordinates (const NavState &g) const
	localCoordinates as required by manifold concept: finds tangent vector between *this and g More...

TangentVector	localCoordinates (const NavState &g, ChartJacobian H1, ChartJacobian H2={}) const
	localCoordinates with optional derivatives More...

TangentVector	logmap (const NavState &g) const

TangentVector	logmap (const NavState &g, ChartJacobian H1, ChartJacobian H2={}) const
	logmap with optional derivatives More...

NavState	retract (const TangentVector &v) const
	retract as required by manifold concept: applies v at *this More...

NavState	retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const
	retract with optional derivatives More...

Static Public Attributes
constexpr static auto	dimension = 9

Static Public Attributes inherited from gtsam::LieGroup< NavState, 9 >
constexpr static auto	dimension

Private Attributes
Rot3	R_
	Rotation nRb, rotates points/velocities in body to points/velocities in nav. More...

Point3	t_
	position n_t, in nav frame More...

Velocity3	v_
	velocity n_v in nav frame More...

Constructors
	NavState ()
	Default constructor. More...

	NavState (const Rot3 &R, const Point3 &t, const Velocity3 &v)
	Construct from attitude, position, velocity. More...

	NavState (const Pose3 &pose, const Velocity3 &v)
	Construct from pose and velocity. More...

	NavState (const Matrix3 &R, const Vector6 &tv)
	Construct from SO(3) and R^6. More...

static NavState	Create (const Rot3 &R, const Point3 &t, const Velocity3 &v, OptionalJacobian< 9, 3 > H1={}, OptionalJacobian< 9, 3 > H2={}, OptionalJacobian< 9, 3 > H3={})
	Named constructor with derivatives. More...

static NavState	FromPoseVelocity (const Pose3 &pose, const Vector3 &vel, OptionalJacobian< 9, 6 > H1={}, OptionalJacobian< 9, 3 > H2={})
	Named constructor with derivatives. More...

Testable
void	print (const std::string &s="") const
	print More...

bool	equals (const NavState &other, double tol=1e-8) const
	equals More...

GTSAM_EXPORT friend std::ostream &	operator<< (std::ostream &os, const NavState &state)
	Output stream operator. More...

Group
NavState	inverse () const
	inverse transformation with derivatives More...

NavState	operator* (const NavState &T) const
	compose syntactic sugar More...

const Rot3 &	rotation () const
	Syntactic sugar. More...

static NavState	Identity ()
	identity for group operation More...

Lie Group
NavState	retract (const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
	retract with optional derivatives More...

Vector9	localCoordinates (const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
	localCoordinates with optional derivatives More...

Matrix9	AdjointMap () const

Vector9	Adjoint (const Vector9 &xi_b, OptionalJacobian< 9, 9 > H_this={}, OptionalJacobian< 9, 9 > H_xib={}) const

static Eigen::Block< Vector9, 3, 1 >	dR (Vector9 &v)

static Eigen::Block< Vector9, 3, 1 >	dP (Vector9 &v)

static Eigen::Block< Vector9, 3, 1 >	dV (Vector9 &v)

static Eigen::Block< const Vector9, 3, 1 >	dR (const Vector9 &v)

static Eigen::Block< const Vector9, 3, 1 >	dP (const Vector9 &v)

static Eigen::Block< const Vector9, 3, 1 >	dV (const Vector9 &v)

static NavState	Expmap (const Vector9 &xi, OptionalJacobian< 9, 9 > Hxi={})

static Vector9	Logmap (const NavState &pose, OptionalJacobian< 9, 9 > Hpose={})

static Matrix9	adjointMap (const Vector9 &xi)

static Vector9	adjoint (const Vector9 &xi, const Vector9 &y, OptionalJacobian< 9, 9 > Hxi={}, OptionalJacobian< 9, 9 > H_y={})

static Matrix9	ExpmapDerivative (const Vector9 &xi)
	Derivative of Expmap. More...

static Matrix9	LogmapDerivative (const NavState &xi)
	Derivative of Logmap. More...

Additional Inherited Members
Public Types inherited from gtsam::LieGroup< NavState, 9 >
typedef OptionalJacobian< N, N >	ChartJacobian

typedef Eigen::Matrix< double, N, N >	Jacobian

typedef Eigen::Matrix< double, N, 1 >	TangentVector

Static Public Member Functions inherited from gtsam::LieGroup< NavState, 9 >
static TangentVector	LocalCoordinates (const NavState &g)
	LocalCoordinates at origin: possible in Lie group because it has an identity. More...

static TangentVector	LocalCoordinates (const NavState &g, ChartJacobian H)
	LocalCoordinates at origin with optional derivative. More...

static NavState	Retract (const TangentVector &v)
	Retract at origin: possible in Lie group because it has an identity. More...

static NavState	Retract (const TangentVector &v, ChartJacobian H)
	Retract at origin with optional derivative. More...

Detailed Description

Navigation state: Pose (rotation, translation) + velocity Following Barrau20icra, this class belongs to the Lie group SE_2(3). This group is also called "double direct isometries”.

NOTE: While Barrau20icra follow a R,v,t order, we use a R,t,v order to maintain backwards compatibility.

Definition at line 38 of file NavState.h.

Constructor & Destructor Documentation

◆ NavState() [1/4]

gtsam::NavState::NavState ( )

inline

Default constructor.

Definition at line 56 of file NavState.h.

◆ NavState() [2/4]

gtsam::NavState::NavState	(	const Rot3 &	R,
		const Point3 &	t,
		const Velocity3 &	v
	)

inline

Construct from attitude, position, velocity.

Definition at line 60 of file NavState.h.

◆ NavState() [3/4]

gtsam::NavState::NavState	(	const Pose3 &	pose,
		const Velocity3 &	v
	)

inline

Construct from pose and velocity.

Definition at line 64 of file NavState.h.

◆ NavState() [4/4]

gtsam::NavState::NavState	(	const Matrix3 &	R,
		const Vector6 &	tv
	)

inline

Construct from SO(3) and R^6.

Definition at line 68 of file NavState.h.

Member Function Documentation

◆ adjoint()

Vector9 gtsam::NavState::adjoint	(	const Vector9 &	xi,
		const Vector9 &	y,
		OptionalJacobian< 9, 9 >	Hxi = `{}`,
		OptionalJacobian< 9, 9 >	H_y = `{}`
	)

static

Action of the adjointMap on a Lie-algebra vector y, with optional derivatives

Definition at line 201 of file NavState.cpp.

◆ Adjoint()

Vector9 gtsam::NavState::Adjoint	(	const Vector9 &	xi_b,
		OptionalJacobian< 9, 9 >	H_this = `{}`,
		OptionalJacobian< 9, 9 >	H_xib = `{}`
	)		const

Apply this NavState's AdjointMap Ad_g to a twist $\xi_b$ , i.e. a body-fixed velocity, transforming it to the spatial frame $\xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b$ Note that H_xib = AdjointMap()

Definition at line 179 of file NavState.cpp.

◆ AdjointMap()

Matrix9 gtsam::NavState::AdjointMap ( ) const

Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame.

Definition at line 168 of file NavState.cpp.

◆ adjointMap()

Matrix9 gtsam::NavState::adjointMap ( const Vector9 & xi )

static

Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 but for the NavState [ad(w,v)] = [w^, zero3; v^, w^]

Definition at line 191 of file NavState.cpp.

◆ attitude()

const Rot3 & gtsam::NavState::attitude ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 48 of file NavState.cpp.

◆ bodyVelocity()

Vector3 gtsam::NavState::bodyVelocity ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 69 of file NavState.cpp.

◆ coriolis()

Vector9 gtsam::NavState::coriolis	(	double	dt,
		const Vector3 &	omega,
		bool	secondOrder = `false`,
		OptionalJacobian< 9, 9 >	H = `{}`
	)		const

Compute tangent space contribution due to Coriolis forces.

Definition at line 372 of file NavState.cpp.

◆ correctPIM()

Vector9 gtsam::NavState::correctPIM	(	const Vector9 &	pim,
		double	dt,
		const Vector3 &	n_gravity,
		const std::optional< Vector3 > &	omegaCoriolis,
		bool	use2ndOrderCoriolis = `false`,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 9 >	H2 = `{}`
	)		const

Correct preintegrated tangent vector with our velocity and rotated gravity, taking into account Coriolis forces if omegaCoriolis is given.

Definition at line 417 of file NavState.cpp.

◆ Create()

NavState gtsam::NavState::Create	(	const Rot3 &	R,
		const Point3 &	t,
		const Velocity3 &	v,
		OptionalJacobian< 9, 3 >	H1 = `{}`,
		OptionalJacobian< 9, 3 >	H2 = `{}`,
		OptionalJacobian< 9, 3 >	H3 = `{}`
	)

static

Named constructor with derivatives.

Definition at line 26 of file NavState.cpp.

◆ dP() [1/2]

static Eigen::Block<const Vector9, 3, 1> gtsam::NavState::dP ( const Vector9 & v )

inlinestatic

Definition at line 175 of file NavState.h.

◆ dP() [2/2]

static Eigen::Block<Vector9, 3, 1> gtsam::NavState::dP ( Vector9 & v )

inlinestatic

Definition at line 166 of file NavState.h.

◆ dR() [1/2]

static Eigen::Block<const Vector9, 3, 1> gtsam::NavState::dR ( const Vector9 & v )

inlinestatic

Definition at line 172 of file NavState.h.

◆ dR() [2/2]

static Eigen::Block<Vector9, 3, 1> gtsam::NavState::dR ( Vector9 & v )

inlinestatic

Definition at line 163 of file NavState.h.

◆ dV() [1/2]

static Eigen::Block<const Vector9, 3, 1> gtsam::NavState::dV ( const Vector9 & v )

inlinestatic

Definition at line 178 of file NavState.h.

◆ dV() [2/2]

static Eigen::Block<Vector9, 3, 1> gtsam::NavState::dV ( Vector9 & v )

inlinestatic

Definition at line 169 of file NavState.h.

◆ equals()

bool gtsam::NavState::equals	(	const NavState &	other,
		double	tol = `1e-8`
	)		const

equals

Definition at line 104 of file NavState.cpp.

◆ Expmap()

NavState gtsam::NavState::Expmap	(	const Vector9 &	xi,
		OptionalJacobian< 9, 9 >	Hxi = `{}`
	)

static

Exponential map at identity - create a NavState from canonical coordinates $ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] $

Definition at line 116 of file NavState.cpp.

◆ ExpmapDerivative()

Matrix9 gtsam::NavState::ExpmapDerivative ( const Vector9 & xi )

static

Derivative of Expmap.

Definition at line 222 of file NavState.cpp.

◆ FromPoseVelocity()

NavState gtsam::NavState::FromPoseVelocity	(	const Pose3 &	pose,
		const Vector3 &	vel,
		OptionalJacobian< 9, 6 >	H1 = `{}`,
		OptionalJacobian< 9, 3 >	H2 = `{}`
	)

static

Named constructor with derivatives.

Definition at line 38 of file NavState.cpp.

◆ Identity()

static NavState gtsam::NavState::Identity ( )

inlinestatic

identity for group operation

Definition at line 140 of file NavState.h.

◆ inverse()

NavState gtsam::NavState::inverse ( ) const

inverse transformation with derivatives

Definition at line 110 of file NavState.cpp.

◆ localCoordinates()

Vector9 gtsam::NavState::localCoordinates	(	const NavState &	g,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 9 >	H2 = `{}`
	)		const

localCoordinates with optional derivatives

Definition at line 291 of file NavState.cpp.

◆ Logmap()

Vector9 gtsam::NavState::Logmap	(	const NavState &	pose,
		OptionalJacobian< 9, 9 >	Hpose = `{}`
	)

static

Log map at identity - return the canonical coordinates $ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] $ of this NavState

Definition at line 140 of file NavState.cpp.

◆ LogmapDerivative()

Matrix9 gtsam::NavState::LogmapDerivative ( const NavState & xi )

static

Derivative of Logmap.

Definition at line 229 of file NavState.cpp.

◆ matrix()

Matrix5 gtsam::NavState::matrix ( ) const

Return matrix group representation, in MATLAB notation: nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1]

Definition at line 80 of file NavState.cpp.

◆ operator*()

NavState gtsam::NavState::operator* ( const NavState & T ) const

inline

compose syntactic sugar

Definition at line 150 of file NavState.h.

◆ pose()

const Pose3 gtsam::NavState::pose ( ) const

inline

Definition at line 90 of file NavState.h.

◆ position()

const Point3 & gtsam::NavState::position ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 55 of file NavState.cpp.

◆ print()

void gtsam::NavState::print ( const std::string & s = "" ) const

print

Definition at line 99 of file NavState.cpp.

◆ quaternion()

Quaternion gtsam::NavState::quaternion ( ) const

inline

Return quaternion. Induces computation in matrix mode.

Definition at line 103 of file NavState.h.

◆ R()

Matrix3 gtsam::NavState::R ( ) const

inline

Return rotation matrix. Induces computation in quaternion mode.

Definition at line 99 of file NavState.h.

◆ retract()

NavState gtsam::NavState::retract	(	const Vector9 &	v,
		OptionalJacobian< 9, 9 >	H1 = `{}`,
		OptionalJacobian< 9, 9 >	H2 = `{}`
	)		const

retract with optional derivatives

Definition at line 264 of file NavState.cpp.

◆ rotation()

const Rot3& gtsam::NavState::rotation ( ) const

inline

Syntactic sugar.

Definition at line 155 of file NavState.h.

◆ t()

Vector3 gtsam::NavState::t ( ) const

inline

Return position as Vector3.

Definition at line 107 of file NavState.h.

◆ update()

NavState gtsam::NavState::update	(	const Vector3 &	b_acceleration,
		const Vector3 &	b_omega,
		const double	dt,
		OptionalJacobian< 9, 9 >	F = `{}`,
		OptionalJacobian< 9, 3 >	G1 = `{}`,
		OptionalJacobian< 9, 3 >	G2 = `{}`
	)		const

Integrate forward in time given angular velocity and acceleration in body frame Uses second order integration for position, returns derivatives except dt.

Definition at line 328 of file NavState.cpp.

◆ v()

const Vector3& gtsam::NavState::v ( ) const

inline

Return velocity as Vector3. Computation-free.

Definition at line 111 of file NavState.h.

◆ velocity()

const Vector3 & gtsam::NavState::velocity ( OptionalJacobian< 3, 9 > H = {} ) const

Definition at line 62 of file NavState.cpp.

Friends And Related Function Documentation

◆ operator<<

GTSAM_EXPORT friend std::ostream& operator<<	(	std::ostream &	os,
		const NavState &	state
	)

friend

Output stream operator.

Definition at line 91 of file NavState.cpp.

Member Data Documentation

◆ dimension

constexpr static auto gtsam::NavState::dimension = 9

inlinestaticconstexpr

Definition at line 49 of file NavState.h.

◆ R_

Rot3 gtsam::NavState::R_

private

Rotation nRb, rotates points/velocities in body to points/velocities in nav.

Definition at line 43 of file NavState.h.

◆ t_

Point3 gtsam::NavState::t_

private

position n_t, in nav frame

Definition at line 44 of file NavState.h.

◆ v_

Velocity3 gtsam::NavState::v_

private

velocity n_v in nav frame

Definition at line 45 of file NavState.h.

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

Classes

Public Member Functions

Static Public Attributes

Private Attributes

Constructors

Testable

Group

Lie Group

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ NavState() [1/4]

◆ NavState() [2/4]

◆ NavState() [3/4]

◆ NavState() [4/4]

Member Function Documentation

◆ adjoint()

◆ Adjoint()

◆ AdjointMap()

◆ adjointMap()

◆ attitude()

◆ bodyVelocity()

◆ coriolis()

◆ correctPIM()

◆ Create()

◆ dP() [1/2]

◆ dP() [2/2]

◆ dR() [1/2]

◆ dR() [2/2]

◆ dV() [1/2]

◆ dV() [2/2]

◆ equals()

◆ Expmap()

◆ ExpmapDerivative()

◆ FromPoseVelocity()

◆ Identity()

◆ inverse()

◆ localCoordinates()

◆ Logmap()

◆ LogmapDerivative()

◆ matrix()

◆ operator*()

◆ pose()

◆ position()

◆ print()

◆ quaternion()

◆ R()

◆ retract()

◆ rotation()

◆ t()

◆ update()

◆ v()

◆ velocity()

Friends And Related Function Documentation

◆ operator<<

Member Data Documentation

◆ dimension

◆ R_

◆ t_

◆ v_