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={}) const
 
const Point3position (OptionalJacobian< 3, 9 > H={}) const
 
const Velocity3velocity (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 Vector3v () const
 Return velocity as Vector3. Computation-free. More...
 
Velocity3 bodyVelocity (OptionalJacobian< 3, 9 > H={}) 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={}) 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
 

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

Manifold

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

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

◆ PositionAndVelocity

Definition at line 49 of file NavState.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
Enumerator
dimension 

Definition at line 45 of file NavState.h.

Constructor & Destructor Documentation

◆ NavState() [1/4]

gtsam::NavState::NavState ( )
inline

Default constructor.

Definition at line 55 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 59 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 63 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 67 of file NavState.h.

Member Function Documentation

◆ 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 214 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 258 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 150 of file NavState.h.

◆ dP() [2/2]

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

Definition at line 141 of file NavState.h.

◆ dR() [1/2]

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

Definition at line 147 of file NavState.h.

◆ dR() [2/2]

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

Definition at line 138 of file NavState.h.

◆ dV() [1/2]

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

Definition at line 153 of file NavState.h.

◆ dV() [2/2]

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

Definition at line 144 of file NavState.h.

◆ equals()

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

equals

Definition at line 101 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.

◆ localCoordinates()

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

localCoordinates with optional derivatives

Definition at line 134 of file NavState.cpp.

◆ matrix()

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

◆ pose()

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

Definition at line 86 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 96 of file NavState.cpp.

◆ quaternion()

Quaternion gtsam::NavState::quaternion ( ) const
inline

Return quaternion. Induces computation in matrix mode.

Definition at line 99 of file NavState.h.

◆ R()

Matrix3 gtsam::NavState::R ( ) const
inline

Return rotation matrix. Induces computation in quaternion mode.

Definition at line 95 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 107 of file NavState.cpp.

◆ t()

Vector3 gtsam::NavState::t ( ) const
inline

Return position as Vector3.

Definition at line 103 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 170 of file NavState.cpp.

◆ v()

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

Return velocity as Vector3. Computation-free.

Definition at line 107 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 88 of file NavState.cpp.

Member Data Documentation

◆ R_

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.

◆ t_

Point3 gtsam::NavState::t_
private

position n_t, in nav frame

Definition at line 40 of file NavState.h.

◆ v_

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 Fri Nov 1 2024 03:51:39