GCOP  1.0
Public Member Functions | Public Attributes
gcop::Body2dTrack Class Reference

#include <body2dtrack.h>

Collaboration diagram for gcop::Body2dTrack:
Collaboration graph
[legend]

List of all members.

Public Member Functions

 Body2dTrack (Body2d<> &sys, int nf, double t0, double tf, double r=25, bool odometry=true, bool extforce=false, bool forces=false)
void Optp (VectorXd &p, const vector< Body2dState > &xs)
void Get (Body2dState &x, double vd, double t) const
void MakeTrue ()
void Add (const Vector3d &u, const Body2dState &x, double h)
void Add2 (const Vector3d &u, const Body2dState &x, double h)

Public Attributes

Body2dsys
 system
double t0
 initial time around track
double tf
 final time around track
double r
 track radius
double w
 track width
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< Body2dStatexs
 sequence of states x=(g,v) (N+1 vector)
vector< Vector3d > us
 sequence of inputs (N vector)
vector< Vector2d > ls
 landmark positions
vector< bool > observed
 was it observed
vector< Body2dStatexos
 unoptimized trajectory
vector< Vector3d > 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,
Vector2d > > > 
Is
 N+1-vector of vectors of visible pose-to-feature indices.
vector< vector< pair< int,
Vector2d > > > 
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< Vector3d > vs
 odometry/gyro velocity measurements
double cp
 noise covariance of poses (assume spherical)
Vector3d cv
 noise covariance of odometry/gyro (assume diagonal)
Vector3d 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

Body2dTrack::Body2dTrack ( Body2d<> &  sys,
int  nf,
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:
sysbody2d system
nfnumber of features (landmarks)
t0start time
tfend time
rradius
odometryis odometry available
extforcetreat constant external force in x-y as a parameter
forcesuse uncertain forces in the cost function

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


Member Function Documentation

void Body2dTrack::Add ( const Vector3d &  u,
const Body2dState x,
double  h 
)
void Body2dTrack::Add2 ( const Vector3d &  u,
const Body2dState x,
double  h 
)

Add a new state/control to estimation vector

Parameters:
ucommanded control
xtrue state (to generate measurements)
htime-step

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

void Body2dTrack::Get ( Body2dState x,
double  vd,
double  t 
) const

References gcop::SE2::Instance(), gcop::SE2::q2g(), r, and tf.

Referenced by Body2dTrack().

References ls, and r.

void Body2dTrack::Optp ( VectorXd &  p,
const vector< Body2dState > &  xs 
)

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

Parameters:
pa vector of feature locations
gsa given vector of poses in SE(2)

References extforce, and Js.


Member Data Documentation

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

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

noise covariance of poses (assume spherical)

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

noise covariance of odometry/gyro (assume diagonal)

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

noise covariance of control forces (assume diagonal)

Referenced by Add(), and gcop::Body2dTrackCost::L().

sensor radius

Referenced by Add(), and Add2().

is there a constant external force that need to be estimated

Referenced by gcop::Body2dTrackCost::L(), Optp(), and gcop::Body2dTrackView::RenderFrame().

is there a constant external force that need to be estimated

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

Referenced by Add(), and Add2().

vector< vector<pair<int, Vector2d> > > gcop::Body2dTrack::Is

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

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

vector< vector<pair<int, Vector2d> > > gcop::Body2dTrack::Js

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

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

vector<Vector2d> gcop::Body2dTrack::ls

was it observed

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

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::Body2dTrackCost::L().

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

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

observed landmark indices (m vector)

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

feature radius

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

track radius

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

system

Referenced by Add(), and Add2().

initial time around track

final time around track

Referenced by Get().

vector<double> gcop::Body2dTrack::ts

sequence of times (N+1 vector)

Referenced by Add(), and Add2().

vector<Vector3d> gcop::Body2dTrack::uos

unoptimized trajectory

Referenced by Add(), Add2(), gcop::Body2dTrackCost::L(), and gcop::Body2dTrackView::RenderFrame().

vector<Vector3d> gcop::Body2dTrack::us

sequence of inputs (N vector)

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

vector<Vector3d> gcop::Body2dTrack::vs

odometry/gyro velocity measurements

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

unoptimized trajectory

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

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

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


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