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

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.

Constructors

 NavState ()
 Default constructor.
 
 NavState (const Rot3 &R, const Point3 &t, const Velocity3 &v)
 Construct from attitude, position, velocity.
 
 NavState (const Pose3 &pose, const Velocity3 &v)
 Construct from pose and velocity.
 
 NavState (const Matrix3 &R, const Vector9 tv)
 Construct from SO(3) and R^6.
 
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.
 
static NavState FromPoseVelocity (const Pose3 &pose, const Vector3 &vel, OptionalJacobian< 9, 6 > H1, OptionalJacobian< 9, 3 > H2)
 Named constructor with derivatives.
 

Testable

GTSAM_EXPORT friend std::ostream & operator<< (std::ostream &os, const NavState &state)
 Output stream operator.
 
void print (const std::string &s="") const
 print
 
bool equals (const NavState &other, double tol=1e-8) const
 equals
 

Manifold

NavState retract (const Vector9 &v, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 9 > H2=boost::none) const
 retract with optional derivatives
 
Vector9 localCoordinates (const NavState &g, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 9 > H2=boost::none) const
 localCoordinates with optional derivatives
 
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
 

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.
 
Quaternion quaternion () const
 Return quaternion. Induces computation in matrix mode.
 
Vector3 t () const
 Return position as Vector3.
 
const Vector3 & v () const
 Return velocity as Vector3. Computation-free.
 
Velocity3 bodyVelocity (OptionalJacobian< 3, 9 > H=boost::none) const
 
Matrix7 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.
 
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
 Integrate forward in time given angular velocity and acceleration in body frame Uses second order integration for position, returns derivatives except dt. More...
 
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.
 
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
 Correct preintegrated tangent vector with our velocity and rotated gravity, taking into account Coriolis forces if omegaCoriolis is given. More...
 

Public Types

enum  { dimension = 9 }
 
typedef std::pair< Point3, Velocity3 > PositionAndVelocity
 

Member Function Documentation

◆ correctPIM()

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.

◆ 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.

Friends And Related Function Documentation

◆ boost::serialization::access

friend class boost::serialization::access
friend

serialization

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