#include <NavState.h>

Public Types
enum	{ dimension = 9 }

typedef std::pair< Point3, Velocity3 >	PositionAndVelocity

Public Member Functions
Component Access
const Rot3 &	attitude (OptionalJacobian< 3, 9 > H=boost::none) const

const Point3 &	position (OptionalJacobian< 3, 9 > H=boost::none) const

const Velocity3 &	velocity (OptionalJacobian< 3, 9 > H=boost::none) 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=boost::none) const

Matrix7	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=boost::none) const
	Compute tangent space contribution due to Coriolis forces. More...

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

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
GTSAM_EXPORT friend std::ostream &	operator<< (std::ostream &os, const NavState &state)
	Output stream operator. More...

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

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

Manifold
NavState	retract (const Vector9 &v, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 9 > H2=boost::none) const
	retract with optional derivatives More...

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

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)

class	boost::serialization::access

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

Detailed Description

Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold

Definition at line 34 of file NavState.h.

Member Typedef Documentation

typedef std::pair<Point3, Velocity3> gtsam::NavState::PositionAndVelocity

Definition at line 49 of file NavState.h.

Member Enumeration Documentation

anonymous enum

Enumerator
dimension

Definition at line 45 of file NavState.h.

Constructor & Destructor Documentation

gtsam::NavState::NavState ( )

inline

Default constructor.

Definition at line 55 of file NavState.h.

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

inline

Construct from attitude, position, velocity.

Definition at line 59 of file NavState.h.

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

inline

Construct from pose and velocity.

Definition at line 63 of file NavState.h.

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

inline

Construct from SO(3) and R^6.

Definition at line 67 of file NavState.h.

Member Function Documentation

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

Definition at line 50 of file NavState.cpp.

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

Definition at line 71 of file NavState.cpp.

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

Compute tangent space contribution due to Coriolis forces.

Definition at line 215 of file NavState.cpp.

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

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

Definition at line 258 of file NavState.cpp.

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 28 of file NavState.cpp.

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

inlinestatic

Definition at line 141 of file NavState.h.

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

inlinestatic

Definition at line 150 of file NavState.h.

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

inlinestatic

Definition at line 138 of file NavState.h.

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

inlinestatic

Definition at line 147 of file NavState.h.

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

inlinestatic

Definition at line 144 of file NavState.h.

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

inlinestatic

Definition at line 153 of file NavState.h.

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

equals

Definition at line 103 of file NavState.cpp.

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 40 of file NavState.cpp.

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

localCoordinates with optional derivatives

Definition at line 135 of file NavState.cpp.

Matrix7 gtsam::NavState::matrix ( ) const

Return matrix group representation, in MATLAB notation: nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] With this embedding in GL(3), matrix product agrees with compose

Definition at line 82 of file NavState.cpp.

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

inline

Definition at line 86 of file NavState.h.

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

Definition at line 57 of file NavState.cpp.

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

print

Definition at line 98 of file NavState.cpp.

Quaternion gtsam::NavState::quaternion ( ) const

inline

Return quaternion. Induces computation in matrix mode.

Definition at line 99 of file NavState.h.

Matrix3 gtsam::NavState::R ( ) const

inline

Return rotation matrix. Induces computation in quaternion mode.

Definition at line 95 of file NavState.h.

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

retract with optional derivatives

Definition at line 109 of file NavState.cpp.

template<class ARCHIVE >

void gtsam::NavState::serialize	(	ARCHIVE &	ar,
		const unsigned	int
	)

inlineprivate

Definition at line 195 of file NavState.h.

Vector3 gtsam::NavState::t ( ) const

inline

Return position as Vector3.

Definition at line 103 of file NavState.h.

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 171 of file NavState.cpp.

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

inline

Return velocity as Vector3. Computation-free.

Definition at line 107 of file NavState.h.

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

Definition at line 64 of file NavState.cpp.

Friends And Related Function Documentation

friend class boost::serialization::access

friend

serialization

Definition at line 193 of file NavState.h.

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

friend

Output stream operator.

Member Data Documentation

Rot3 gtsam::NavState::R_

private

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

Definition at line 39 of file NavState.h.

Point3 gtsam::NavState::t_

private

position n_t, in nav frame

Definition at line 40 of file NavState.h.

Velocity3 gtsam::NavState::v_

private

velocity n_v in nav frame

Definition at line 41 of file NavState.h.

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

Public Types

Public Member Functions

Private Attributes

Constructors

Testable

Manifold

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation