|
GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
#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. | |
| 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={}) 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. | |
| 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 |
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 Vector6 &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 | |
| void | print (const std::string &s="") const |
| print | |
| bool | equals (const NavState &other, double tol=1e-8) const |
| equals | |
| GTSAM_EXPORT friend std::ostream & | operator<< (std::ostream &os, const NavState &state) |
| Output stream operator. | |
Manifold | |
| NavState | retract (const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const |
| retract with optional derivatives | |
| Vector9 | localCoordinates (const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) 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) |
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
| 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.
| 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
| 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.
1.8.13