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

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.

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

friend class boost::serialization::access
friend

serialization

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