gcop::Body2dGraph Class Reference

#include <body2dgraph.h>

Collaboration diagram for gcop::Body2dGraph:

List of all members.

Public Member Functions
	Body2dGraph (Body2d<> &sys, int N, int nf, bool odometry=true, bool extforce=false, bool forces=false)
void	Optp (VectorXd &p, const vector< Body2dState > &xs)
Static Public Member Functions
static void	Synthesize (Body2dGraph &pgt, Body2dGraph &pgn, double tf)
static void	Synthesize2 (Body2dGraph &pgt, Body2dGraph &pgn, double tf)
static void	Synthesize3 (Body2dGraph &pgt, Body2dGraph &pgn, double tf)
Public Attributes
Body2d &	sys
	system
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
	optimize over forces
Vector3d	fext
	unknown external force
vector< double >	ts
	sequence of times (N+1 vector)
vector< Body2dState >	xs
	sequence of states x=(g,v) (N+1 vector)
vector< Vector3d >	us
	sequence of inputs (N vector)
VectorXd	p
	feature positions (2*nf vector) and external force parameters (2 vector)
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< 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-graph 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

Body2dGraph::Body2dGraph	(	Body2d<> &	sys,
		int	N,
		int	nf,
		bool	odometry = true,
		bool	extforce = false,
		bool	forces = false
	)

Pose-landmark graph

Parameters:

sys	body2d system
N	number of trajectory segments
nf	number of features (landmarks)
odometry	is odometry available
extforce	treat constant external force in x-y as a parameter
forces	use uncertain forces in the cost function

---

Member Function Documentation

void Body2dGraph::Optp	(	VectorXd &	p,
		const vector< Body2dState > &	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.

Referenced by Synthesize(), Synthesize2(), and Synthesize3().

void Body2dGraph::Synthesize	(	Body2dGraph &	pgt,
		Body2dGraph &	pgn,
		double	tf
	)		[static]

Create true and noisy synthetic pose graphs

Parameters:

pgt	true pose graph
pgn	noisy pose graph
tf	total trajectory time

References gcop::SE2::cay(), cp, gcop::SE2::Instance(), Is, Js, Optp(), p, gcop::random_normal(), ts, us, and xs.

void Body2dGraph::Synthesize2	(	Body2dGraph &	pgt,
		Body2dGraph &	pgn,
		double	tf
	)		[static]

Create true and noisy synthetic pose graphs

Parameters:

pgt	true pose graph
pgn	noisy pose graph
tf	total trajectory time

References gcop::SE2::cay(), cp, extforce, gcop::Body2d< c >::force, gcop::SE2::Instance(), Is, Js, Optp(), p, gcop::random_normal(), sys, ts, us, vs, and xs.

void Body2dGraph::Synthesize3	(	Body2dGraph &	pgt,
		Body2dGraph &	pgn,
		double	tf
	)		[static]

Create true and noisy synthetic pose graphs

Parameters:

pgt	true pose graph
pgn	noisy pose graph
tf	total trajectory time

References gcop::SE2::cay(), cp, cv, cw, extforce, fext, gcop::Body2d< c >::force, gcop::Body2d< c >::I, gcop::SE2::Instance(), Is, Js, Optp(), p, gcop::random_normal(), gcop::Body2d< c >::Step(), sys, ts, us, vs, and xs.

---

Member Data Documentation

double gcop::Body2dGraph::cp

noise covariance of poses (assume spherical)

Referenced by gcop::Body2dSlamCost::Lp(), Synthesize(), Synthesize2(), and Synthesize3().

Vector3d gcop::Body2dGraph::cv

noise covariance of odometry/gyro (assume diagonal)

Referenced by gcop::Body2dSlamCost::Lp(), and Synthesize3().

Vector3d gcop::Body2dGraph::cw

noise covariance of control forces (assume diagonal)

Referenced by gcop::Body2dSlamCost::Lp(), and Synthesize3().

bool gcop::Body2dGraph::extforce

is there a constant external force that need to be estimated

Referenced by gcop::Body2dSlam::Body2dSlam(), gcop::Body2dSlamCost::Lp(), Optp(), gcop::Body2dGraphView::RenderFrame(), Synthesize2(), and Synthesize3().

Vector3d gcop::Body2dGraph::fext

unknown external force

Referenced by Synthesize3().

bool gcop::Body2dGraph::forces

optimize over forces

Referenced by gcop::Body2dSlamCost::Lp().

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

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

Referenced by gcop::Body2dSlamCost::Lp(), Synthesize(), Synthesize2(), and Synthesize3().

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

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

Referenced by Optp(), gcop::Body2dGraphView::RenderFrame(), Synthesize(), Synthesize2(), and Synthesize3().

bool gcop::Body2dGraph::odometry

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

Referenced by gcop::Body2dSlamCost::Lp().

VectorXd gcop::Body2dGraph::p

feature positions (2*nf vector) and external force parameters (2 vector)

Referenced by gcop::Body2dSlam::Body2dSlam(), gcop::Body2dGraphView::RenderFrame(), Synthesize(), Synthesize2(), and Synthesize3().

Body2d& gcop::Body2dGraph::sys

system

Referenced by gcop::Body2dSlam::Body2dSlam(), Synthesize2(), and Synthesize3().

vector<double> gcop::Body2dGraph::ts

sequence of times (N+1 vector)

Referenced by gcop::Body2dSlam::Body2dSlam(), Synthesize(), Synthesize2(), and Synthesize3().

vector<Vector3d> gcop::Body2dGraph::us

sequence of inputs (N vector)

Referenced by gcop::Body2dSlam::Body2dSlam(), gcop::Body2dSlamCost::Lp(), gcop::Body2dGraphView::RenderFrame(), Synthesize(), Synthesize2(), and Synthesize3().

vector<Vector3d> gcop::Body2dGraph::vs

odometry/gyro velocity measurements

Referenced by gcop::Body2dSlamCost::Lp(), Synthesize2(), and Synthesize3().

vector<Body2dState> gcop::Body2dGraph::xs

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

Referenced by gcop::Body2dSlam::Body2dSlam(), gcop::Body2dGraphView::RenderFrame(), Synthesize(), Synthesize2(), and Synthesize3().

---

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

• lib/algos/body2dgraph.h
• lib/algos/body2dgraph.cc