Base class for all robots defined in the module rtslam. More...

Detailed Description

Base class for all robots defined in the module rtslam.

Definition at line 93 of file robotAbstract.hpp.

#include <robotAbstract.hpp>

Inheritance diagram for jafar::rtslam::RobotAbstract:

Public Types
enum	type_enum { ODOMETRY, CONSTANT_VELOCITY, CONSTANT_MOTION_MODEL, INERTIAL, CENTERED_CONSTANT_VELOCITY }
Public Member Functions
virtual bool	registerRobotQuantity (Quantity quantity)
	robot or environment quantities to be estimated, that are shared by sensors and predefined
virtual ublas::range	allocateSensorQuantity (size_t size)
	sensor quantities to be estimated (biases, ...) that are defined by the sensor
	ENABLE_ACCESS_TO_CHILDREN (SensorAbstract, Sensor, sensor)
	RobotAbstract (const map_ptr_t &_mapPtr)
void	construct (const map_ptr_t &_mapPtr, const size_t _size_state, const size_t _size_control, const size_t _size_pert)
	Remote constructor from remote map and size of state and control vectors.
void	setPoseStd (double x, double y, double z, double roll, double pitch, double yaw, double xStd, double yStd, double zStd, double rollStd, double pitchStd, double yawStd, bool degrees=false)
void	setPositionStd (double x, double y, double z, double xStd, double yStd, double zStd)
void	setOrientationStd (double roll, double pitch, double yaw, double rollStd, double pitchStd, double yawStd, bool degrees=false)
virtual std::string	categoryName () const
void	setId ()
void	setRobotPose (jblas::vec6 const &pose_euler, bool degrees=false)
	Set the pose of the true robot in the slam robot frame The Slam robot frame is the IMU frame for inertial slam, and as defined by the sensorPoses otherwise.
void	setInitialPose (double x, double y, double z, double roll, double pitch, double yaw, double xStd, double yStd, double zStd, double rollStd, double pitchStd, double yawStd, bool degrees=false)
	Set the initial pose of the real robot in the exported frame.
void	setOrigin (double x, double y, double z)
	Set the origin of the exported frame in the absolute frame used by absolute sensors (ie GPS) If you don't use if the first absolute pos sensor reading will compute it and set it, which is fine if this happens at the very beginning, but if it happens later the drift between the start position and when it happens won't be corrected.
void	setInitialOrientation (double roll, double pitch, double yaw, double rollStd, double pitchStd, double yawStd, bool degrees=false)
	Set the absolute real robot initial orientation.
void	getCurrentPose (double time, jblas::vec &x, jblas::sym_mat &P)
	Get the absolute real robot current full pose, in euler roll/pitch/yaw, at the given time (must be roughly current time or a little bit in the future, cannot be before last sensor integration)
template<class VEC , class SYM_MAT >
void	slamPoseToRobotPose (VEC &slam_x, SYM_MAT &slam_P, jblas::vec &robot_x, jblas::sym_mat &robot_P) const
	return pose and uncertainty of the robot frame (and normalizes slam's q)
virtual size_t	mySize ()=0
virtual size_t	mySize_control ()=0
virtual size_t	mySize_perturbation ()=0
void	set_control (const vec &c)
void	set_perturbation (const Perturbation &_pert)
void	setHardwareEstimator (hardware::hardware_sensorprop_ptr_t hardwareEstimatorPtr_)
void	init (const vec &_u, const vec &_U)
	Initialize the state, affect SLAM filter.
void	init (const vec &_u)
void	move ()
	Move one step ahead, affect SLAM filter.
void	move (const vec &_u)
	Move one step ahead, affect SLAM filter.
bool	computeControls (double time1, double time2, jblas::mat &controls, bool release) const
virtual bool	move (double time)
void	move_extrapolate ()
	Move without changing the state, for extrapolation Thread safe.
void	move_extrapolate (double time)
void	reinit_extrapolate ()
void	move_fake (double time)
	move without moving (without doing slam), just to release the data
void	computeStatePerturbation ()
	Compute robot process noise Q in state space.
void	computeStatePerturbation_extrapol ()
void	applyConstraints ()
virtual void	writeLogHeader (kernel::DataLogger &log) const
	Implements this method calling repeatidly log methods.
virtual void	writeLogData (kernel::DataLogger &log) const
	Implements this method calling repeatidly log methods.
void	stopDumping ()
void	setHistoryManager (history_manager_ptr_t hm)
Static Public Member Functions
static size_t	size_control ()
static size_t	size_perturbation ()
Public Attributes
type_enum	type
sensor_ptr_t	last_updated
hardware::hardware_sensorprop_ptr_t	hardwareEstimatorPtr
history_manager_ptr_t	historyManager
Gaussian	pose
	Robot Gaussian pose.
vec	control
	Control vector.
Perturbation	perturbation
	Perturbation Gaussian vector.
bool	constantPerturbation
	Constant perturbation flag.
kernel::Timestamp	self_time
	Current estimation time.
double	dt_or_dx
	Sampling time or any other relevant increment (e.g. odometry is not time-driven but distance-driven)
jblas::mat	controls
jblas::mat	XNEW_x
	Jacobian wrt state.
jblas::mat	XNEW_pert
	Jacobian wrt perturbation.
jblas::sym_mat	Q
	Process noise covariances matrix in state space, Q = XNEW_pert * perturbation.P * trans(XNEW_pert);.
jblas::vec	robot_pose
	the pose of the true robot in the slam robot frame, for exported position
jblas::vec	start_pos_export
	start position of the robot in the exported frame
jblas::vec	start_pos_abs
	start position of the robot in the absolute frame, ie the frame of absolute sensors such as GPS (UTM)
bool	is_start_pos_abs_init
	is start_pos_abs initialized?
boost::mutex	mutex_extrapol
bool	extrapol_up_to_date
double	self_time_extrapol
double	self_time_extrapol_init
double	self_time_extrapol_inc
jblas::vec	state_extrapol_x
jblas::vec	state_extrapol_x_init
jblas::vec	state_extrapol_x_inc
jblas::sym_mat	state_extrapol_P
jblas::sym_mat	state_extrapol_P_init
jblas::sym_mat	state_extrapol_P_inc
vec	control_extrapol
Perturbation	perturbation_extrapol
double	dt_or_dx_extrapol
jblas::mat	controls_extrapol
jblas::mat	XNEW_x_extrapol
jblas::mat	XNEW_pert_extrapol
jblas::sym_mat	Q_extrapol
Protected Member Functions
virtual void	move_func (const vec &_x, const vec &_u, const vec &_n, const double _dt, vec &_xnew, mat &_XNEW_x, mat &_XNEW_pert, unsigned tempSet=0) const =0
	Move one step ahead.
virtual void	init_func (const vec &_x, const vec &_u, const vec &_U, vec &_xnew)
	Initialize the robot state with the average previous values of control.
virtual void	init_func (const vec &_x, const vec &_u, vec &_xnew)
Protected Attributes
bool	constructed
Static Protected Attributes
static IdFactory	robotIds
Friends
ostream &	operator<< (ostream &s, RobotAbstract const &rob)

Member Function Documentation

void jafar::rtslam::RobotAbstract::computeStatePerturbation ( )

Compute robot process noise Q in state space.

This function is called by move() at each iteration if constantPerturbation is false. It performs the operation:

Q = XNEW_pert * perturbation.P() * XNEW_pert',

where XNEW_pert and perturbation.P() must have been already computed.

void jafar::rtslam::RobotAbstract::construct	(	const map_ptr_t &	_mapPtr,
		const size_t	_size_state,
		const size_t	_size_control,
		const size_t	_size_pert
	)

Remote constructor from remote map and size of state and control vectors.

Parameters:

_mapPtr	a pointer to the map.
_size_state	the size of the robot state vector
_size_control	the size of the control vector
_size_pert	the size of the perturbation vector

Referenced by allocateSensorQuantity(), and registerRobotQuantity().

virtual void jafar::rtslam::RobotAbstract::init_func	(	const vec &	_x,
		const vec &	_u,
		const vec &	_U,
		vec &	_xnew
	)		`[inline, protected, virtual]`

Initialize the robot state with the average previous values of control.

You may optionnaly implement this function in derived classes.

This function can initialize differently the robot state, given the average control input on a past period of time, if possible.

Reimplemented in jafar::rtslam::RobotInertial.

Definition at line 445 of file robotAbstract.hpp.

Referenced by init().

void jafar::rtslam::RobotAbstract::move ( )

Move one step ahead, affect SLAM filter.

This function updates the full state and covariances matrix of the robot plus the cross-variances with all other map objects.

Referenced by move().

void jafar::rtslam::RobotAbstract::move ( const vec & _u ) [inline]

Move one step ahead, affect SLAM filter.

This function updates the full state and covariances matrix of the robot plus the cross-variances with all other map objects.

Definition at line 358 of file robotAbstract.hpp.

References JFR_ASSERT, and move().

virtual void jafar::rtslam::RobotAbstract::move_func	(	const vec &	_x,
		const vec &	_u,
		const vec &	_n,
		const double	_dt,
		vec &	_xnew,
		mat &	_XNEW_x,
		mat &	_XNEW_pert,
		unsigned	tempSet = `0`
	)		const `[protected, pure virtual]`

Move one step ahead.

Implement this function in every derived class.

This function predicts the robot state one step of length _dt ahead in time, according to the current state _x, the control input _u and the time interval _dt. It computes the new state and the convenient Jacobian matrices.

In clase the flag constantPerturbation is set to true, the matrix _XNEW_pert is not computed.

See also:: Documentation of the constantPerturbation flag.

Parameters:

_x	the current state vector
_u	the control vector
_n	the perturbation vector
_dt	the time interval
_xnew	the new state
_XNEW_x	the Jacobian of _xnew wrt _x
_XNEW_pert	the Jacobian of _xnew wrt _n
tempSet	the id of the set of temporary variables that must be used (stored in the class)

Implemented in jafar::rtslam::RobotInertial, jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >, jafar::rtslam::RobotConstantVelocity, jafar::rtslam::RobotOdometry, and jafar::rtslam::RobotCenteredConstantVelocity.

virtual void jafar::rtslam::RobotAbstract::writeLogData ( kernel::DataLogger & log ) const [virtual]

Implements this method calling repeatidly log methods.

You should use writeData() or writeDataVector().

Implements jafar::kernel::DataLoggable.

Reimplemented in jafar::rtslam::RobotInertial, jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >, jafar::rtslam::RobotConstantVelocity, and jafar::rtslam::RobotOdometry.

virtual void jafar::rtslam::RobotAbstract::writeLogHeader ( kernel::DataLogger & log ) const [virtual]

Implements this method calling repeatidly log methods.

You should use writeComment(), writeLegend() or writeLegendTokens().

Implements jafar::kernel::DataLoggable.

Reimplemented in jafar::rtslam::RobotInertial, jafar::rtslam::RobotConstantMotionModel< LIN_ORDER, ANG_ORDER >, jafar::rtslam::RobotConstantVelocity, and jafar::rtslam::RobotOdometry.

Member Data Documentation

bool jafar::rtslam::RobotAbstract::constantPerturbation

Constant perturbation flag.

Flag for indicating that the state perturbation Q is constant and should not be computed at each iteration.

In case this flag wants to be set to true , the user must consider computing the constant Q immediately after construction time. This can be done in three ways:

define a function member setup(..args..) in the derived class to compute the Jacobian XNEW_pert, and enter the appropriate perturbation.P() value. Then call computeStatePerturbation(), which will compute Q from P() and XNEW_pert.
define a function member setup(..args..) in the derived class to enter the matrix Q directly.

In any of the above cases, call your setup() function immediately after the constructor.

Define a constructor that accepts a number of parameters relevant to your perturbation levels, and perform all the above operations to obtain Q inside the constructor body.

Definition at line 194 of file robotAbstract.hpp.

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

modules/rtslam/include/rtslam/robotAbstract.hpp

Detailed Description

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Static Protected Attributes

Friends

Member Function Documentation

Member Data Documentation