#include <PoseRTV.h>

Inheritance diagram for gtsam::PoseRTV:

Collaboration 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)

	PoseRTV (const Vector &v)

const Pose3 &	pose () const

const Velocity3 &	v () const

const Point3 &	t () const

const Rot3 &	R () const

const Point3 &	translation () const

const Rot3 &	rotation () const

const Velocity3 &	velocity () const

Vector	vector () const

Vector	translationVec () const

const Velocity3 &	velocityVec () 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={}, 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.

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

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)

Protected Types
typedef ProductLieGroup< Pose3, Velocity3 >	Base

typedef OptionalJacobian< 9, 9 >	ChartJacobian

enum

enum

Group
ProductLieGroup	operator* (const ProductLieGroup &other) const

ProductLieGroup	inverse () const

ProductLieGroup	compose (const ProductLieGroup &g) const

ProductLieGroup	between (const ProductLieGroup &g) const

static ProductLieGroup	Identity ()

typedef multiplicative_group_tag	group_flavor

Manifold
size_t	dim () const

ProductLieGroup	retract (const TangentVector &v, ChartJacobian H1={}, ChartJacobian H2={}) const

TangentVector	localCoordinates (const ProductLieGroup &g, ChartJacobian H1={}, ChartJacobian H2={}) const

static size_t	Dim ()

enum

typedef Eigen::Matrix< double, dimension, 1 >	TangentVector

Lie Group
typedef Eigen::Matrix< double, dimension, dimension >	Jacobian

typedef Eigen::Matrix< double, dimension1, dimension1 >	Jacobian1

typedef Eigen::Matrix< double, dimension2, dimension2 >	Jacobian2

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

static ProductLieGroup	Expmap (const TangentVector &v, ChartJacobian Hv={})

static TangentVector	Logmap (const ProductLieGroup &p, ChartJacobian Hp={})

static TangentVector	LocalCoordinates (const ProductLieGroup &p, ChartJacobian Hp={})

Detailed Description

Robot state for use with IMU measurements

contains translation, translational velocity and rotation TODO(frank): Alex should deprecate/move to project

Constructor & Destructor Documentation

◆ PoseRTV() [1/2]

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

◆ PoseRTV() [2/2]

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

explicit

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

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]

◆ 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

◆ range()

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

range between translations

◆ RRTMbn()

static 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

◆ RRTMnb()

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

static

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

◆ 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

◆ translationIntegration() [1/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

◆ translationIntegration() [2/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

◆ 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 file:

/home/docs/checkouts/readthedocs.org/user_builds/gtsam-jlblanco-docs/checkouts/latest/gtsam_unstable/dynamics/PoseRTV.h

Public Member Functions

Static Public Member Functions

Protected Types

Group

Manifold

Lie Group