Public Types | Private Attributes | List of all members
gtsam::NavState Class Reference

#include <NavState.h>

Public Types

enum  { dimension = 9 }
 
typedef std::pair< Point3, Velocity3PositionAndVelocity
 

Public Member Functions

Component Access
const Rot3attitude (OptionalJacobian< 3, 9 > H=boost::none) const
 
const Point3position (OptionalJacobian< 3, 9 > H=boost::none) const
 
const Velocity3velocity (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 Vector3v () 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

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:


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