#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={}) const |
const Point3 & | position (OptionalJacobian< 3, 9 > H={}) const |
const Velocity3 & | velocity (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 Vector3 & | v () 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) |
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.
typedef std::pair<Point3, Velocity3> gtsam::NavState::PositionAndVelocity |
Definition at line 49 of file NavState.h.
anonymous enum |
Enumerator | |
---|---|
dimension |
Definition at line 45 of file NavState.h.
|
inline |
Default constructor.
Definition at line 55 of file NavState.h.
Construct from attitude, position, velocity.
Definition at line 59 of file NavState.h.
Construct from pose and velocity.
Definition at line 63 of file NavState.h.
|
inline |
Construct from SO(3) and R^6.
Definition at line 67 of file NavState.h.
const Rot3 & gtsam::NavState::attitude | ( | OptionalJacobian< 3, 9 > | H = {} | ) | const |
Definition at line 48 of file NavState.cpp.
Vector3 gtsam::NavState::bodyVelocity | ( | OptionalJacobian< 3, 9 > | H = {} | ) | const |
Definition at line 69 of file NavState.cpp.
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.
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.
|
static |
Named constructor with derivatives.
Definition at line 26 of file NavState.cpp.
|
inlinestatic |
Definition at line 150 of file NavState.h.
|
inlinestatic |
Definition at line 141 of file NavState.h.
|
inlinestatic |
Definition at line 147 of file NavState.h.
|
inlinestatic |
Definition at line 138 of file NavState.h.
|
inlinestatic |
Definition at line 153 of file NavState.h.
|
inlinestatic |
Definition at line 144 of file NavState.h.
equals
Definition at line 101 of file NavState.cpp.
|
static |
Named constructor with derivatives.
Definition at line 38 of file NavState.cpp.
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.
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.
|
inline |
Definition at line 86 of file NavState.h.
const Point3 & gtsam::NavState::position | ( | OptionalJacobian< 3, 9 > | H = {} | ) | const |
Definition at line 55 of file NavState.cpp.
void gtsam::NavState::print | ( | const std::string & | s = "" | ) | const |
Definition at line 96 of file NavState.cpp.
|
inline |
Return quaternion. Induces computation in matrix mode.
Definition at line 99 of file NavState.h.
|
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 = {} , |
||
OptionalJacobian< 9, 9 > | H2 = {} |
||
) | const |
retract with optional derivatives
Definition at line 107 of file NavState.cpp.
|
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 170 of file NavState.cpp.
|
inline |
Return velocity as Vector3. Computation-free.
Definition at line 107 of file NavState.h.
const Vector3 & gtsam::NavState::velocity | ( | OptionalJacobian< 3, 9 > | H = {} | ) | const |
Definition at line 62 of file NavState.cpp.
|
friend |
Output stream operator.
Definition at line 88 of file NavState.cpp.
|
private |
Rotation nRb, rotates points/velocities in body to points/velocities in nav.
Definition at line 39 of file NavState.h.
|
private |
position n_t, in nav frame
Definition at line 40 of file NavState.h.
|
private |
velocity n_v in nav frame
Definition at line 41 of file NavState.h.