gtsam  4.0.0
gtsam
gtsam::PoseRTV Class Reference

Detailed Description

Robot state for use with IMU measurements.

  • contains translation, translational velocity and rotation TODO(frank): Alex should deprecate/move to project
+ Inheritance diagram for gtsam::PoseRTV:

Public Member Functions

 PoseRTV (const Point3 &t, const Rot3 &rot, const Velocity3 &vel)
 
 PoseRTV (const Rot3 &rot, const Point3 &t, const Velocity3 &vel)
 
 PoseRTV (const Point3 &t)
 
 PoseRTV (const Pose3 &pose, const Velocity3 &vel)
 
 PoseRTV (const Pose3 &pose)
 
 PoseRTV (const Base &base)
 
 PoseRTV (double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz)
 build from components - useful for data files
 
 PoseRTV (const Vector &v)
 build from single vector - useful for Matlab - in RtV format
 
const Pose3pose () const
 
const Velocity3v () const
 
const Point3t () const
 
const Rot3R () const
 
const Point3translation () const
 
const Rot3rotation () const
 
const Velocity3velocity () const
 
Vector vector () const
 
Vector translationVec () const
 
const Velocity3velocityVec () const
 
bool equals (const PoseRTV &other, double tol=1e-6) const
 
void print (const std::string &s="") const
 
measurement functions
double range (const PoseRTV &other, OptionalJacobian< 1, 9 > H1=boost::none, OptionalJacobian< 1, 9 > H2=boost::none) const
 range between translations
 
IMU-specific
PoseRTV planarDynamics (double vel_rate, double heading_rate, double max_accel, double dt) const
 Dynamics integrator for ground robots Always move from time 1 to time 2.
 
PoseRTV flyingDynamics (double pitch_rate, double heading_rate, double lift_control, double dt) const
 Simulates flying robot with simple flight model Integrates state x1 -> x2 given controls x1 = {p1, r1, v1}, x2 = {p2, r2, v2}, all in global coordinates. More...
 
PoseRTV generalDynamics (const Vector &accel, const Vector &gyro, double dt) const
 General Dynamics update - supply control inputs in body frame.
 
Vector6 imuPrediction (const PoseRTV &x2, double dt) const
 Dynamics predictor for both ground and flying robots, given states at 1 and 2 Always move from time 1 to time 2. More...
 
Point3 translationIntegration (const Rot3 &r2, const Velocity3 &v2, double dt) const
 predict measurement and where Point3 for x2 should be, as a way of enforcing a velocity constraint This version splits out the rotation and velocity for x2
 
Point3 translationIntegration (const PoseRTV &x2, double dt) const
 predict measurement and where Point3 for x2 should be, as a way of enforcing a velocity constraint This version takes a full PoseRTV, but ignores the existing translation for x2
 
Vector translationIntegrationVec (const PoseRTV &x2, double dt) const
 
PoseRTV transformed_from (const Pose3 &trans, ChartJacobian Dglobal=boost::none, OptionalJacobian< 9, 6 > Dtrans=boost::none) const
 Apply transform to this pose, with optional derivatives equivalent to: local = trans.transformFrom(global, Dtrans, Dglobal) More...
 
- Public Member Functions inherited from gtsam::ProductLieGroup< Pose3, Velocity3 >
 ProductLieGroup ()
 Default constructor yields identity.
 
 ProductLieGroup (const Pose3 &g, const Velocity3 &h)
 
 ProductLieGroup (const Base &base)
 
ProductLieGroup inverse (ChartJacobian D) const
 
ProductLieGroup compose (const ProductLieGroup &other, ChartJacobian H1, ChartJacobian H2=boost::none) const
 
ProductLieGroup between (const ProductLieGroup &other, ChartJacobian H1, ChartJacobian H2=boost::none) const
 
ProductLieGroup expmap (const TangentVector &v) const
 
TangentVector logmap (const ProductLieGroup &g) const
 
size_t dim () const
 
ProductLieGroup retract (const TangentVector &v, ChartJacobian H1=boost::none, ChartJacobian H2=boost::none) const
 
TangentVector localCoordinates (const ProductLieGroup &g, ChartJacobian H1=boost::none, ChartJacobian H2=boost::none) const
 
ProductLieGroup operator * (const ProductLieGroup &other) const
 
ProductLieGroup inverse () const
 
ProductLieGroup compose (const ProductLieGroup &g) const
 
ProductLieGroup between (const ProductLieGroup &g) const
 

Static Public Member Functions

Utility functions
static Matrix RRTMbn (const Vector3 &euler)
 RRTMbn - Function computes the rotation rate transformation matrix from body axis rates to euler angle (global) rates.
 
static Matrix RRTMbn (const Rot3 &att)
 
static Matrix RRTMnb (const Vector3 &euler)
 RRTMnb - Function computes the rotation rate transformation matrix from euler angle rates to body axis rates.
 
static Matrix RRTMnb (const Rot3 &att)
 
- Static Public Member Functions inherited from gtsam::ProductLieGroup< Pose3, Velocity3 >
static ProductLieGroup Expmap (const TangentVector &v, ChartJacobian Hv=boost::none)
 
static TangentVector Logmap (const ProductLieGroup &p, ChartJacobian Hp=boost::none)
 
static size_t Dim ()
 
static ProductLieGroup identity ()
 

Protected Types

typedef ProductLieGroup< Pose3, Velocity3Base
 
typedef OptionalJacobian< 9, 9 > ChartJacobian
 
- Protected Types inherited from gtsam::ProductLieGroup< Pose3, Velocity3 >
enum  
 
enum  
 
typedef Eigen::Matrix< double, dimension, dimension > Jacobian
 
typedef Eigen::Matrix< double, dimension1, dimension1 > Jacobian1
 
typedef Eigen::Matrix< double, dimension2, dimension2 > Jacobian2
 

Friends

class boost::serialization::access
 Serialization function.
 

Additional Inherited Members

- Public Types inherited from gtsam::ProductLieGroup< Pose3, Velocity3 >
enum  
 
typedef Eigen::Matrix< double, dimension, 1 > TangentVector
 
typedef OptionalJacobian< dimension, dimension > ChartJacobian
 
typedef multiplicative_group_tag group_flavor
 

Member Function Documentation

◆ flyingDynamics()

PoseRTV gtsam::PoseRTV::flyingDynamics ( double  pitch_rate,
double  heading_rate,
double  lift_control,
double  dt 
) const

Simulates flying robot with simple flight model Integrates state x1 -> x2 given controls x1 = {p1, r1, v1}, x2 = {p2, r2, v2}, all in global coordinates.

Returns
x2

◆ imuPrediction()

Vector6 gtsam::PoseRTV::imuPrediction ( const PoseRTV x2,
double  dt 
) const

Dynamics predictor for both ground and flying robots, given states at 1 and 2 Always move from time 1 to time 2.

Returns
imu measurement, as [accel, gyro]

◆ transformed_from()

PoseRTV gtsam::PoseRTV::transformed_from ( const Pose3 trans,
ChartJacobian  Dglobal = boost::none,
OptionalJacobian< 9, 6 >  Dtrans = boost::none 
) const

Apply transform to this pose, with optional derivatives equivalent to: local = trans.transformFrom(global, Dtrans, Dglobal)

Note: the transform jacobian convention is flipped

◆ translationIntegrationVec()

Vector gtsam::PoseRTV::translationIntegrationVec ( const PoseRTV x2,
double  dt 
) const
inline
Returns
a vector for Matlab compatibility

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