#include <PoseRTV.h>
Public Member Functions | |
bool | equals (const PoseRTV &other, double tol=1e-6) const |
const Pose3 & | pose () 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 Rot3 & | R () const |
const Rot3 & | rotation () const |
const Point3 & | t () const |
const Point3 & | translation () const |
Vector | translationVec () const |
const Velocity3 & | v () const |
Vector | vector () const |
const Velocity3 & | velocity () const |
const Velocity3 & | velocityVec () 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, Velocity3 > | Base |
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 |
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 |
Robot state for use with IMU measurements
|
protected |
|
protected |
|
inlineexplicit |
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.
|
explicit |
build from single vector - useful for Matlab - in RtV format
Definition at line 46 of file PoseRTV.cpp.
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
Definition at line 85 of file PoseRTV.cpp.
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.
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
Definition at line 133 of file PoseRTV.cpp.
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.
void gtsam::PoseRTV::print | ( | const std::string & | s = "" | ) | const |
Definition at line 52 of file PoseRTV.cpp.
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.
Definition at line 220 of file PoseRTV.cpp.
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.
Definition at line 236 of file PoseRTV.cpp.
RRTMnb - Function computes the rotation rate transformation matrix from euler angle rates to body axis rates
Definition at line 225 of file PoseRTV.cpp.
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.
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.
Vector gtsam::PoseRTV::vector | ( | ) | const |
Definition at line 37 of file PoseRTV.cpp.
|
inline |