GCOP: gcop::Body3dTrack Class Reference

GCOP 1.0

#include <body3dtrack.h>

Collaboration diagram for gcop::Body3dTrack:

Collaboration graph

List of all members.

Public Member Functions
	Body3dTrack (Body3d<> &sys, int nf, double vd0, double t0, double tf, double r=25, bool odometry=true, bool extforce=false, bool forces=false)
void	Optp (VectorXd &p, const vector< Body3dState > &xs)
void	Get (Body3dState &x, double vd, double t) const
void	MakeTrue ()
void	Add (const Vector6d &u, const Body3dState &x, double h)
void	Add2 (const Vector6d &u, const Body3dState &x, double h)
Public Attributes
Body3d &	sys
	system
double	t0
	initial time around track
double	tf
	final time around track
double	r
	track radius
double	w
	track width
double	h
	track height
double	dmax
	sensor radius
bool	odometry
	is there odometry available (this means that the velocity part of the state will be regarded as available measurements)
bool	extforce
	is there a constant external force that need to be estimated
bool	forces
	is there a constant external force that need to be estimated
vector< double >	ts
	sequence of times (N+1 vector)
vector< Body3dState >	xs
	sequence of states x=(g,v) (N+1 vector)
vector< Vector6d >	us
	sequence of inputs (N vector)
vector< Vector3d >	ls
	landmark positions
vector< bool >	observed
	was it observed
vector< Body3dState >	xos
	unoptimized trajectory
vector< Vector6d >	uos
	unoptimized trajectory
bool	init
VectorXd	p
	observed landmark positions (2*m vector) and external force parameters (2 vector)
vector< int >	pis
	observed landmark indices (m vector)
double	pr
	feature radius
vector< vector< pair< int, Vector3d > > >	Is
	N+1-vector of vectors of visible pose-to-feature indices.
vector< vector< pair< int, Vector3d > > >	Js
	nf-vector of vectors of feature-to-pose indices indices
vector< int >	cis
	if observed then this should map into the corresponding value in p
vector< Vector6d >	vs
	odometry/gyro velocity measurements
double	cp
	noise covariance of poses (assume spherical)
Vector6d	cv
	noise covariance of odometry/gyro (assume diagonal)
Vector6d	cw
	noise covariance of control forces (assume diagonal)

Detailed Description

Pose-track in 2D. This is the simplese possible definition assuming point (unprojected) features e.g. from stereo/laser scans with uniform spherical covariance. The framework can be easily extended to any general 2d/3d non-uniform or projected features as well.

Constructor & Destructor Documentation

Body3dTrack::Body3dTrack	(	Body3d<> &	sys,
		int	nf,
		double	vd0,
		double	t0,
		double	tf,
		double	r = `25`,
		bool	odometry = `true`,
		bool	extforce = `false`,
		bool	forces = `false`
	)

Pose graph (using simulated features around a circular track)

Parameters:

sys	body3d system
nf	number of features (landmarks)
t0	start time
tf	end time
r	radius
odometry	is odometry available
extforce	treat constant external force in x-y as a parameter
forces	use uncertain forces in the cost function

References cv, cw, Get(), vs, xos, and xs.

Member Function Documentation

void Body3dTrack::Add	(	const Vector6d &	u,
		const Body3dState &	x,
		double	h
	)

References cis, cp, cv, cw, dmax, h, init, Is, Js, ls, observed, odometry, p, pis, gcop::random_normal(), gcop::Body3d< c >::Step(), sys, ts, uos, us, vs, xos, and xs.

void Body3dTrack::Add2	(	const Vector6d &	u,
		const Body3dState &	x,
		double	h
	)

Add a new state/control to estimation vector

Parameters:

u	commanded control
x	true state (to generate measurements)
h	time-step

References cis, cp, cv, dmax, h, init, Is, Js, ls, observed, odometry, p, pis, gcop::random_normal(), gcop::Body3d< c >::Step(), sys, ts, uos, us, vs, xos, and xs.

void Body3dTrack::Get	(	Body3dState &	x,
		double	vd,
		double	t
	)		const

References r, and tf.

Referenced by Body3dTrack().

void Body3dTrack::MakeTrue ( )

References h, ls, r, and w.

void Body3dTrack::Optp	(	VectorXd &	p,
		const vector< Body3dState > &	xs
	)

Given a sequence of poses gs, compute the optimal feature locations p

Parameters:

p	a vector of feature locations
gs	a given vector of poses in SE(2)

References extforce, and Js.

Member Data Documentation

vector<int> gcop::Body3dTrack::cis

if observed then this should map into the corresponding value in p

Referenced by Add(), Add2(), and gcop::Body3dTrackCost::L().

double gcop::Body3dTrack::cp

noise covariance of poses (assume spherical)

Referenced by Add(), Add2(), and gcop::Body3dTrackCost::L().

Vector6d gcop::Body3dTrack::cv

noise covariance of odometry/gyro (assume diagonal)

Referenced by Add(), Add2(), Body3dTrack(), and gcop::Body3dTrackCost::L().

Vector6d gcop::Body3dTrack::cw

noise covariance of control forces (assume diagonal)

Referenced by Add(), Body3dTrack(), and gcop::Body3dTrackCost::L().

double gcop::Body3dTrack::dmax

sensor radius

Referenced by Add(), and Add2().

bool gcop::Body3dTrack::extforce

is there a constant external force that need to be estimated

Referenced by gcop::Body3dTrackCost::L(), Optp(), and gcop::Body3dTrackView::RenderFrame().

bool gcop::Body3dTrack::forces

is there a constant external force that need to be estimated

Referenced by gcop::Body3dTrackCost::L().

double gcop::Body3dTrack::h

track height

Referenced by Add(), Add2(), and MakeTrue().

bool gcop::Body3dTrack::init

Referenced by Add(), and Add2().

vector< vector<pair<int, Vector3d> > > gcop::Body3dTrack::Is

N+1-vector of vectors of visible pose-to-feature indices.

Referenced by Add(), Add2(), and gcop::Body3dTrackCost::L().

vector< vector<pair<int, Vector3d> > > gcop::Body3dTrack::Js

nf-vector of vectors of feature-to-pose indices indices

Referenced by Add(), Add2(), Optp(), and gcop::Body3dTrackView::RenderFrame().

vector<Vector3d> gcop::Body3dTrack::ls

landmark positions

Referenced by Add(), Add2(), MakeTrue(), and gcop::Body3dTrackView::RenderFrame().

vector<bool> gcop::Body3dTrack::observed

was it observed

Referenced by Add(), Add2(), and gcop::Body3dTrackView::RenderFrame().

bool gcop::Body3dTrack::odometry

is there odometry available (this means that the velocity part of the state will be regarded as available measurements)

Referenced by Add(), Add2(), and gcop::Body3dTrackCost::L().

VectorXd gcop::Body3dTrack::p

observed landmark positions (2*m vector) and external force parameters (2 vector)

Referenced by Add(), Add2(), and gcop::Body3dTrackView::RenderFrame().

vector<int> gcop::Body3dTrack::pis

observed landmark indices (m vector)

Referenced by Add(), Add2(), and gcop::Body3dTrackView::RenderFrame().

double gcop::Body3dTrack::pr

feature radius

Referenced by gcop::Body3dTrackView::RenderFrame().

double gcop::Body3dTrack::r

track radius

Referenced by Get(), MakeTrue(), and gcop::Body3dTrackView::RenderFrame().

Body3d& gcop::Body3dTrack::sys

system

Referenced by Add(), and Add2().

double gcop::Body3dTrack::t0

initial time around track

double gcop::Body3dTrack::tf

final time around track

Referenced by Get().

vector<double> gcop::Body3dTrack::ts

sequence of times (N+1 vector)

Referenced by Add(), and Add2().

vector<Vector6d> gcop::Body3dTrack::uos

unoptimized trajectory

Referenced by Add(), Add2(), and gcop::Body3dTrackCost::L().

vector<Vector6d> gcop::Body3dTrack::us

sequence of inputs (N vector)

Referenced by Add(), Add2(), and gcop::Body3dTrackView::RenderFrame().

vector<Vector6d> gcop::Body3dTrack::vs

odometry/gyro velocity measurements

Referenced by Add(), Add2(), Body3dTrack(), and gcop::Body3dTrackCost::L().

double gcop::Body3dTrack::w

track width

Referenced by MakeTrue(), and gcop::Body3dTrackView::RenderFrame().

vector<Body3dState> gcop::Body3dTrack::xos

unoptimized trajectory

Referenced by Add(), Add2(), and Body3dTrack().

vector<Body3dState> gcop::Body3dTrack::xs

sequence of states x=(g,v) (N+1 vector)

Referenced by Add(), Add2(), Body3dTrack(), and gcop::Body3dTrackView::RenderFrame().

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

lib/systems/body3dtrack.h
lib/systems/body3dtrack.cc