#include <PoseRTV.h>

Inheritance diagram for gtsam::PoseRTV:

Public Member Functions
bool	equals (const PoseRTV &other, double tol=1e-6) const

const Pose3 &	pose () const

	PoseRTV ()

	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)

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=boost::none, OptionalJacobian< 1, 9 > H2=boost::none) 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=boost::none, OptionalJacobian< 9, 6 > Dtrans=boost::none) const

Public Member Functions inherited from gtsam::ProductLieGroup< Pose3, Velocity3 >
	ProductLieGroup ()
	Default constructor yields identity. More...

	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)

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=boost::none)

static TangentVector	Logmap (const ProductLieGroup &p, ChartJacobian Hp=boost::none)

static TangentVector	LocalCoordinates (const ProductLieGroup &p, ChartJacobian Hp=boost::none)

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

Private Member Functions
template<class Archive >
void	serialize (Archive &ar, const unsigned int)

Friends
class	boost::serialization::access

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

Detailed Description

Robot state for use with IMU measurements

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

Definition at line 23 of file PoseRTV.h.

Member Typedef Documentation

typedef ProductLieGroup<Pose3,Velocity3> gtsam::PoseRTV::Base

protected

Definition at line 26 of file PoseRTV.h.

typedef OptionalJacobian<9, 9> gtsam::PoseRTV::ChartJacobian

protected

Definition at line 27 of file PoseRTV.h.

Constructor & Destructor Documentation

gtsam::PoseRTV::PoseRTV ( )

inline

Definition at line 32 of file PoseRTV.h.

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

inline

Definition at line 33 of file PoseRTV.h.

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

inline

Definition at line 35 of file PoseRTV.h.

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

inlineexplicit

Definition at line 37 of file PoseRTV.h.

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

inline

Definition at line 39 of file PoseRTV.h.

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

inlineexplicit

Definition at line 41 of file PoseRTV.h.

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

inline

Definition at line 45 of file PoseRTV.h.

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.

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

explicit

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

Member Function Documentation

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

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

Returns: x2

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

Returns: imu measurement, as [accel, gyro]

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.

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

inline

Definition at line 56 of file PoseRTV.h.

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

Definition at line 52 of file PoseRTV.cpp.

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

inline

Definition at line 59 of file PoseRTV.h.

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

range between translations

Definition at line 169 of file PoseRTV.cpp.

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

inline

Definition at line 63 of file PoseRTV.h.

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.

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

static

Definition at line 220 of file PoseRTV.cpp.

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.

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

static

Definition at line 236 of file PoseRTV.cpp.

template<class Archive >

void gtsam::PoseRTV::serialize	(	Archive &	ar,
		const unsigned	int
	)

inlineprivate

Definition at line 163 of file PoseRTV.h.

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

inline

Definition at line 58 of file PoseRTV.h.

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

Definition at line 182 of file PoseRTV.cpp.

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

inline

Definition at line 62 of file PoseRTV.h.

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.

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.

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.

Vector gtsam::PoseRTV::translationVec ( ) const

inline

Definition at line 69 of file PoseRTV.h.

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

inline

Definition at line 57 of file PoseRTV.h.

Vector gtsam::PoseRTV::vector ( ) const

Definition at line 37 of file PoseRTV.cpp.

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

inline

Definition at line 64 of file PoseRTV.h.

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

inline

Definition at line 70 of file PoseRTV.h.

Friends And Related Function Documentation

friend class boost::serialization::access

friend

Serialization function

Definition at line 161 of file PoseRTV.h.

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

Public Member Functions

Static Public Member Functions

Protected Types

Private Member Functions

Friends

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation