#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={}, 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 Pose3 &g, const Velocity3 &h)

	ProductLieGroup (const Base &base)

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

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

◆ Base

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

protected

Definition at line 26 of file PoseRTV.h.

◆ ChartJacobian

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

protected

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 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.

◆ RRTMbn() [2/2]

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

static

Definition at line 220 of file PoseRTV.cpp.

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

◆ RRTMnb() [2/2]

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

static

Definition at line 236 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 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.

◆ 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

Definition at line 124 of file PoseRTV.h.

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

Public Member Functions

Static Public Member Functions

Protected Types

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Base

◆ ChartJacobian

Constructor & Destructor Documentation

◆ PoseRTV() [1/9]

◆ PoseRTV() [2/9]

◆ PoseRTV() [3/9]

◆ PoseRTV() [4/9]

◆ PoseRTV() [5/9]

◆ PoseRTV() [6/9]

◆ PoseRTV() [7/9]

◆ PoseRTV() [8/9]

◆ PoseRTV() [9/9]

Member Function Documentation

◆ equals()

◆ flyingDynamics()

◆ generalDynamics()

◆ imuPrediction()

◆ planarDynamics()

◆ pose()

◆ print()

◆ R()

◆ range()

◆ rotation()

◆ RRTMbn() [1/2]

◆ RRTMbn() [2/2]

◆ RRTMnb() [1/2]

◆ RRTMnb() [2/2]

◆ t()

◆ transformed_from()

◆ translation()

◆ translationIntegration() [1/2]

◆ translationIntegration() [2/2]

◆ translationIntegrationVec()

◆ translationVec()

◆ v()

◆ vector()

◆ velocity()

◆ velocityVec()