Public Member Functions | Protected Types | List of all members
gtsam::PoseRTV Class Reference

#include <PoseRTV.h>

Inheritance diagram for gtsam::PoseRTV:
Inheritance graph
[legend]

Public Member Functions

bool equals (const PoseRTV &other, double tol=1e-6) const
 
const Pose3pose () const
 
 PoseRTV ()
 
 PoseRTV (const Base &base)
 
 PoseRTV (const Point3 &t)
 
 PoseRTV (const Point3 &t, const Rot3 &rot, const Velocity3 &vel)
 
 PoseRTV (const Pose3 &pose)
 
 PoseRTV (const Pose3 &pose, const Velocity3 &vel)
 
 PoseRTV (const Rot3 &rot, const Point3 &t, const Velocity3 &vel)
 
 PoseRTV (const Vector &v)
 
 PoseRTV (double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz)
 
void print (const std::string &s="") const
 
const Rot3R () const
 
const Rot3rotation () const
 
const Point3t () const
 
const Point3translation () const
 
Vector translationVec () const
 
const Velocity3v () const
 
Vector vector () const
 
const Velocity3velocity () const
 
const Velocity3velocityVec () const
 
measurement functions
double range (const PoseRTV &other, OptionalJacobian< 1, 9 > H1={}, OptionalJacobian< 1, 9 > H2={}) const
 
IMU-specific
PoseRTV planarDynamics (double vel_rate, double heading_rate, double max_accel, double dt) const
 
PoseRTV flyingDynamics (double pitch_rate, double heading_rate, double lift_control, double dt) const
 
PoseRTV generalDynamics (const Vector &accel, const Vector &gyro, double dt) const
 General Dynamics update - supply control inputs in body frame. More...
 
Vector6 imuPrediction (const PoseRTV &x2, double dt) const
 
Point3 translationIntegration (const Rot3 &r2, const Velocity3 &v2, double dt) const
 
Point3 translationIntegration (const PoseRTV &x2, double dt) const
 
Vector translationIntegrationVec (const PoseRTV &x2, double dt) const
 
PoseRTV transformed_from (const Pose3 &trans, ChartJacobian Dglobal={}, OptionalJacobian< 9, 6 > Dtrans={}) const
 
- Public Member Functions inherited from gtsam::ProductLieGroup< Pose3, Velocity3 >
 ProductLieGroup ()
 Default constructor yields identity. More...
 
 ProductLieGroup (const Base &base)
 
 ProductLieGroup (const Pose3 &g, const Velocity3 &h)
 
ProductLieGroup inverse (ChartJacobian D) const
 
ProductLieGroup compose (const ProductLieGroup &other, ChartJacobian H1, ChartJacobian H2={}) const
 
ProductLieGroup between (const ProductLieGroup &other, ChartJacobian H1, ChartJacobian H2={}) const
 
ProductLieGroup expmap (const TangentVector &v) const
 
TangentVector logmap (const ProductLieGroup &g) const
 
size_t dim () const
 
ProductLieGroup retract (const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={}) const
 
TangentVector localCoordinates (const ProductLieGroup &g, ChartJacobian H1={}, ChartJacobian H2={}) 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)
 
static Matrix RRTMbn (const Rot3 &att)
 
static Matrix RRTMnb (const Vector3 &euler)
 
static Matrix RRTMnb (const Rot3 &att)
 
- Static Public Member Functions inherited from gtsam::ProductLieGroup< Pose3, Velocity3 >
static ProductLieGroup Expmap (const TangentVector &v, ChartJacobian Hv={})
 
static TangentVector Logmap (const ProductLieGroup &p, ChartJacobian Hp={})
 
static TangentVector LocalCoordinates (const ProductLieGroup &p, ChartJacobian Hp={})
 
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, dimensionJacobian
 
typedef Eigen::Matrix< double, dimension1, dimension1Jacobian1
 
typedef Eigen::Matrix< double, dimension2, dimension2Jacobian2
 

Additional Inherited Members

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

Detailed Description

Robot state for use with IMU measurements

Definition at line 23 of file PoseRTV.h.

Member Typedef Documentation

◆ Base

Definition at line 26 of file PoseRTV.h.

◆ ChartJacobian

Definition at line 27 of file PoseRTV.h.

Constructor & Destructor Documentation

◆ PoseRTV() [1/9]

gtsam::PoseRTV::PoseRTV ( )
inline

Definition at line 32 of file PoseRTV.h.

◆ PoseRTV() [2/9]

gtsam::PoseRTV::PoseRTV ( const Point3 t,
const Rot3 rot,
const Velocity3 vel 
)
inline

Definition at line 33 of file PoseRTV.h.

◆ PoseRTV() [3/9]

gtsam::PoseRTV::PoseRTV ( const Rot3 rot,
const Point3 t,
const Velocity3 vel 
)
inline

Definition at line 35 of file PoseRTV.h.

◆ PoseRTV() [4/9]

gtsam::PoseRTV::PoseRTV ( const Point3 t)
inlineexplicit

Definition at line 37 of file PoseRTV.h.

◆ PoseRTV() [5/9]

gtsam::PoseRTV::PoseRTV ( const Pose3 pose,
const Velocity3 vel 
)
inline

Definition at line 39 of file PoseRTV.h.

◆ PoseRTV() [6/9]

gtsam::PoseRTV::PoseRTV ( const Pose3 pose)
inlineexplicit

Definition at line 41 of file PoseRTV.h.

◆ PoseRTV() [7/9]

gtsam::PoseRTV::PoseRTV ( const Base base)
inline

Definition at line 45 of file PoseRTV.h.

◆ PoseRTV() [8/9]

gtsam::PoseRTV::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

Definition at line 24 of file PoseRTV.cpp.

◆ PoseRTV() [9/9]

gtsam::PoseRTV::PoseRTV ( const Vector v)
explicit

build from single vector - useful for Matlab - in RtV format

Member Function Documentation

◆ equals()

bool gtsam::PoseRTV::equals ( const PoseRTV other,
double  tol = 1e-6 
) const

Definition at line 46 of file PoseRTV.cpp.

◆ 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

Definition at line 85 of file PoseRTV.cpp.

◆ generalDynamics()

PoseRTV gtsam::PoseRTV::generalDynamics ( const Vector accel,
const Vector gyro,
double  dt 
) const

General Dynamics update - supply control inputs in body frame.

Definition at line 118 of file PoseRTV.cpp.

◆ 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]

Definition at line 133 of file PoseRTV.cpp.

◆ planarDynamics()

PoseRTV gtsam::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

Definition at line 60 of file PoseRTV.cpp.

◆ pose()

const Pose3& gtsam::PoseRTV::pose ( ) const
inline

Definition at line 56 of file PoseRTV.h.

◆ print()

void gtsam::PoseRTV::print ( const std::string &  s = "") const

Definition at line 52 of file PoseRTV.cpp.

◆ R()

const Rot3& gtsam::PoseRTV::R ( ) const
inline

Definition at line 59 of file PoseRTV.h.

◆ range()

double gtsam::PoseRTV::range ( const PoseRTV other,
OptionalJacobian< 1, 9 >  H1 = {},
OptionalJacobian< 1, 9 >  H2 = {} 
) const

range between translations

Definition at line 169 of file PoseRTV.cpp.

◆ rotation()

const Rot3& gtsam::PoseRTV::rotation ( ) const
inline

Definition at line 63 of file PoseRTV.h.

◆ RRTMbn() [1/2]

Matrix gtsam::PoseRTV::RRTMbn ( const Rot3 att)
static

Definition at line 220 of file PoseRTV.cpp.

◆ RRTMbn() [2/2]

Matrix gtsam::PoseRTV::RRTMbn ( const Vector3 euler)
static

RRTMbn - Function computes the rotation rate transformation matrix from body axis rates to euler angle (global) rates

Definition at line 208 of file PoseRTV.cpp.

◆ RRTMnb() [1/2]

Matrix gtsam::PoseRTV::RRTMnb ( const Rot3 att)
static

Definition at line 236 of file PoseRTV.cpp.

◆ RRTMnb() [2/2]

Matrix gtsam::PoseRTV::RRTMnb ( const Vector3 euler)
static

RRTMnb - Function computes the rotation rate transformation matrix from euler angle rates to body axis rates

Definition at line 225 of file PoseRTV.cpp.

◆ t()

const Point3& gtsam::PoseRTV::t ( ) const
inline

Definition at line 58 of file PoseRTV.h.

◆ transformed_from()

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

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

Note: the transform jacobian convention is flipped

Definition at line 182 of file PoseRTV.cpp.

◆ translation()

const Point3& gtsam::PoseRTV::translation ( ) const
inline

Definition at line 62 of file PoseRTV.h.

◆ translationIntegration() [1/2]

Point3 gtsam::PoseRTV::translationIntegration ( const PoseRTV x2,
double  dt 
) const
inline

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

Definition at line 124 of file PoseRTV.h.

◆ translationIntegration() [2/2]

Point3 gtsam::PoseRTV::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

Definition at line 161 of file PoseRTV.cpp.

◆ translationIntegrationVec()

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

Definition at line 129 of file PoseRTV.h.

◆ translationVec()

Vector gtsam::PoseRTV::translationVec ( ) const
inline

Definition at line 69 of file PoseRTV.h.

◆ v()

const Velocity3& gtsam::PoseRTV::v ( ) const
inline

Definition at line 57 of file PoseRTV.h.

◆ vector()

Vector gtsam::PoseRTV::vector ( ) const

Definition at line 37 of file PoseRTV.cpp.

◆ velocity()

const Velocity3& gtsam::PoseRTV::velocity ( ) const
inline

Definition at line 64 of file PoseRTV.h.

◆ velocityVec()

const Velocity3& gtsam::PoseRTV::velocityVec ( ) const
inline

Definition at line 70 of file PoseRTV.h.


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


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:16:24