This class solves SLAM problem using the EKF-based academic solution. More...

Detailed Description

This class solves SLAM problem using the EKF-based academic solution.

The pose of the robot is represented using Euler angle in a vector $ [x,y,z,yaw,pitch,roll] $ .

Todo:

add robot model, also with constraints (for attitude quaternion for instance)

free space management in the state vector ! (when the removeLandmark function is used)

Definition at line 44 of file slamEkf.hpp.

#include <slamEkf.hpp>

Inheritance diagram for jafar::slam::SlamEkf:

[legend]

List of all members.

Classes
class	Stats
Public Types
enum	MergeMapNumber { MAP_1, MAP_2 }
	Merge two maps, and creates a new one. More...
typedef std::map< unsigned int, BaseFeature * >	FeaturesMapType
typedef std::map< unsigned int, PoseCopy * >	PoseCopyContType
typedef std::map< unsigned int, BaseRobot * >	RobotsMapType
Public Member Functions
	SlamEkf (std::size_t sizeMax_, std::size_t sizeRobotState_, std::size_t sizeRobotPose_)
	Constructor.
virtual	~SlamEkf ()
	Destructor.
virtual void	init (boost::posix_time::time_duration const &curTime)
	clear the map and the robot pose
std::size_t	sizeRobotPose () const
	Return robot's pose size.
std::size_t	sizeRobotState () const
	Returns robot's state size.
jblas::vec_range const &	refPose (int _robotId=0) const
	Return robot pose.
jblas::sym_mat_range const &	refPoseCov (int _robotId=0) const
	Return robot pose covariances matrix.
void	enableAutoLogFeatures (jafar::kernel::DataLogger &masterLogger)
jafar::filter::BlockExtendedKalmanFilter &	getFilter ()
	get the SLAM filter
jafar::filter::BlockExtendedKalmanFilter const &	getFilter () const
	get the SLAM filter
FeaturesMapType const &	getMap () const
	Return the map of features.
jafar::geom::T3D const &	refToRobot () const
jafar::geom::T3D const &	robotToRef () const
void	setRefToRobot (jafar::geom::T3DEuler const &refToRobot_)
void	setRobotPose (jblas::vec const &robotPose, int _robotId=0)
void	setRobotPoseCov (jblas::vec const &robotPose, jblas::sym_mat const &robotPoseCov, int _robotId=0)
FeatureObserveModel &	getFeatureObserveModel (unsigned int sensorId=0) const
unsigned int	getCurrentFrameIndex () const
unsigned int	getPrevFrameIndex () const
void	setMapManager (AbstractMapManager *mapManager_)
void	setDeleteSensors (bool)
	Set wether the SlamEkf can delete FeatureObserveModel pass as argument of setSensor .
void	setSensor (FeatureObserveModel *model, int sensorId=0)
	Add a sensor with its feature model.
void	setRobotZ (double z, double zStdDev, int _robotId=0)
void	setRobotPitch (double pitch, double pitchStdDev, int _robotId=0)
void	setRobotRoll (double roll, double rollStdDev, int _robotId=0)
virtual void	setRobotToSensor (jblas::vec const &robotToSensor, int sensorId=0)
	Set the robotToSensor transformation of sensor sensorId.
const jblas::vec &	robotToSensor (int sensorId=0) const
virtual void	setRobotToSensor (jblas::vec const &robotToSensor, jblas::vec const &robotToSensorStdDev, int sensorId=0)
	Set the robotToSensor transformation along with its uncertainty of sensor sensorId.
virtual void	addEventListener (SlamEventListener &listener)
virtual void	removeEventListener (SlamEventListener &listener)
bool	hasFeature (unsigned int id, unsigned int _robotId=0) const
virtual bool	isFeatureKnown (unsigned int id) const
BaseFeature &	getFeature (unsigned int id)
BaseFeature const &	getFeature (unsigned int id) const
virtual void	removeFeature (unsigned int id)
PoseCopy const &	copyCurrentRefPoseToMap (unsigned int id)
	This method copy the current reference pose to the map, along with the appropriate correlations.
PoseCopy const &	getPoseCopy (unsigned int id) const
	Return the current estimate of the poseCopy id.
void	removePoseCopy (unsigned int id)
	Remove poseCopy id.
void	writePoseCopyToFile (unsigned int id, std::string const &dir)
	Write poseCopy id in a t3d file.
void	writeAllPoseCopyToFile (std::string const &dir)
double	getRobotPoseUncertaintyLevel (unsigned int robotId_=0) const
void	getRobotPose (jafar::geom::T3DEuler &pose, unsigned int _robotId=0) const
void	transform (const geom::T3DEuler &_transform)
	This function will transform the position of the landmarks, and of the robot with the given transformation.
std::size_t	sizeMap () const
double	getMapUncertaintyLevel () const
void	beginBrowseFeatures ()
bool	hasNextFeature ()
BaseFeature *	nextFeature ()
BaseFeature *	feature (unsigned int id)
virtual void	predict (unsigned int _robotId, jblas::vec const &u)
	Compute the prediction for the movement of a robot.
BaseRobot *	robot (unsigned int _id)
const BaseRobot *	robot (unsigned int) const
virtual void	addRobot (BaseRobot *)
	Add a new BaseRobot in the map.
virtual void	addRobot (BaseRobot *, const jblas::vec &_robotState, const jblas::sym_mat &_robotStateCov)
virtual void	processObservations (unsigned int frameIndex_, std::list< Observation * > const &knownObs, std::list< Observation * > const &newObs, unsigned int robotId_=0, int events_=SlamEventListener::ALL)
virtual void	processObservations (unsigned int frameIndex_, std::vector< Observation * > const &knownObs, std::vector< Observation * > const &newObs, unsigned int robotId_=0, int events_=SlamEventListener::ALL)
template<class SequenceType >
void	processObservations (unsigned int frameIndex_, SequenceType const &allObs, unsigned int robotId_=0, int events_=SlamEventListener::ALL)
template<class segmentFeatureObserveModel >
void	computeExtObs (int robotId_, SegmentFeature &segmentFeature_, segmentFeatureObserveModel &obsModel, jblas::vec &zPredExt1, jblas::sym_mat &zPredExt1Cov, jblas::vec &zPredExt2, jblas::sym_mat &zPredExt2Cov)
	Compute Extremites Observation /.
template<class segmentFeatureObserveModel >
void	computeExtObs (int robotId_, SegmentIDFeature &segmentFeature_, segmentFeatureObserveModel &obsModel, jblas::vec &zPredExt1, jblas::sym_mat &zPredExt1Cov, jblas::vec &zPredExt2, jblas::sym_mat &zPredExt2Cov)
template<class segmentFeatureObserveModel >
void	computeExtObs (int robotId_, SegmentAnchorFeature &segmentFeature_, segmentFeatureObserveModel &obsModel, jblas::vec &zPredExt1, jblas::sym_mat &zPredExt1Cov, jblas::vec &zPredExt2, jblas::sym_mat &zPredExt2Cov)
Static Public Member Functions
static SlamEkf *	mergeMap (const SlamEkf map1, const SlamEkf map2, const jblas::vec &map1ToMap2, const jblas::mat &map1ToMap2Cov, const std::map< unsigned int, MergeMapNumber > &_chooseRobotPose)
Public Attributes
FeaturesMapType	featuresMap
	Current map of features.
PoseCopyContType	poseCopyCont
	estimated local frames
RobotsMapType	robotsMap
	List of robots inside the map.
FeaturesMapType::iterator	browseFeaturesIt
Protected Types
typedef std::list < SlamEventListener * >	SlamEventListenersList
typedef std::map< unsigned int, FeatureObserveModel * >	FeatureObserveModelsContType
Protected Member Functions
virtual void	observeNewFeature (Observation *obs_)
	This method adds a new feature to the map.
virtual void	observeKnownFeature (Observation *obs_)
	This method performs observation update to a feature in the map.
void	observeKnownFeature (Observation const &obs, BaseFeature &feature, BaseFeatureObserveModel &model)
	This method performs observation update to a feature in the map.
void	applyConstraints (BaseFeature &feature)
	apply constraints of feature
void	fixFeature (BaseFeature &feature)
	Make feature satisfy a constraint.
void	copyCurrentPoseAt (std::size_t index, int _robotId=0)
	Copy current robot pose at given map location and size.
void	writePoseCopyToFile (PoseCopy const &pc, std::string const &dir)
virtual void	writeLogHeader (jafar::kernel::DataLogger &log) const
	Implements this method calling repeatidly log methods.
virtual void	writeLogData (jafar::kernel::DataLogger &log) const
	Implements this method calling repeatidly log methods.
virtual void	writeLogStats (jafar::kernel::DataLogger &log) const
	write stats at the end of the log
void	logFeature (BaseFeature &f)
Static Protected Member Functions
static BaseFeature *	featureFactory (unsigned int id, BaseFeatureObserveModel &model)
static BaseFeature *	featureFactory (unsigned int id, FeatureModel &model, std::size_t sizeObs, Observation::ObservationType typeObs)
Protected Attributes
bool	m_deleteSensors
unsigned int	currentFrameIndex
unsigned int	prevFrameIndex
jafar::filter::BlockExtendedKalmanFilter	filter
double	mahalanobisDistanceToRemoveLandmark
std::size_t	m_sizeRobotState
std::size_t	m_sizeRobotPose
AbstractMapManager *	mapManager
SlamEventListenersList	slamEventListeners
bool	enableInitCoordinateFrame
	FIXME not implemented yet.
FeatureObserveModelsContType	featureObserveModels
	Observe models for simple features.
class jafar::slam::SlamEkf::Stats	stats
geom::T3D *	m_refToRobot
	relation between the robot frame and the slam (ref) frame
geom::T3D *	m_robotToRef
unsigned short int	nbInconsistentUpdates
unsigned short int	nbNewLandmarks
unsigned short int	nbObservedLandmarks
long	updateElapsedTime
jafar::kernel::DataLogger *	p_masterLogger
Private Member Functions
void	initFeatureState (BaseFeature &feature, FeatureObserveModel &observeModel, Observation const &obs)
	init value of feature state, covariance and cross covariance.
	SlamEkf (const SlamEkf &)
SlamEkf &	operator= (const SlamEkf &)
template<class SequenceType1 , class SequenceType2 >
void	processObservationsImpl (unsigned int frameIndex_, SequenceType1 const &knownObs, SequenceType2 const &newObs, unsigned int robotId_, int events_)
Friends
class	DefaultMapManager
class	LocalMapManager
class	GlobalMapManager
std::ostream &	operator<< (std::ostream &s, const SlamEkf &slam_)

Member Enumeration Documentation

enum jafar::slam::SlamEkf::MergeMapNumber

Merge two maps, and creates a new one.

Returns:: the Mahalanobis for this observation (already matched); the Mahalanobis distance between feature_ and obs_

Definition at line 636 of file slamEkf.hpp.

Member Function Documentation

virtual void jafar::slam::SlamEkf::addRobot ( BaseRobot * ) [virtual]

Add a new BaseRobot in the map.

The function will initialize the state of the robot in the filter.

Implements jafar::slam::BaseSlam.

Reimplemented in jafar::slam::BearingOnlySlam.

template<class segmentFeatureObserveModel >

void jafar::slam::SlamEkf::computeExtObs	(	int	robotId_,
		SegmentFeature &	segmentFeature_,
		segmentFeatureObserveModel &	obsModel,
		jblas::vec &	zPredExt1,
		jblas::sym_mat &	zPredExt1Cov,
		jblas::vec &	zPredExt2,
		jblas::sym_mat &	zPredExt2Cov
	)		`[inline]`

Compute Extremites Observation /.

Parameters:

_robotId	Robot Identifier /
segmentFeature_	Segment Landmark. CAUTION this function it is only needed in the case of segments. /
zPredExt1	and
zPredExt2	Extremities Observation /
zPredExt1Cov	and
zPredExt2Cov	Extremities Observation Covariances

Definition at line 477 of file slamEkf.hpp.

BaseFeature* jafar::slam::SlamEkf::feature ( unsigned int id )

Returns:: the feature of the given id , or 0, if there is no feature id

static SlamEkf* jafar::slam::SlamEkf::mergeMap	(	const SlamEkf *	map1,
		const SlamEkf *	map2,
		const jblas::vec &	map1ToMap2,
		const jblas::mat &	map1ToMap2Cov,
		const std::map< unsigned int, MergeMapNumber > &	_chooseRobotPose
	)		`[static]`

Parameters:

_chooseRobotPose a map that indicates the merge which map should be used to select the robot pose

virtual void jafar::slam::SlamEkf::observeNewFeature ( Observation * obs_ ) [protected, virtual]

This method adds a new feature to the map.

A new feature can be initialized only with an observation with the same dimension as the state of the feature.

Reimplemented in jafar::slam::BearingOnlySlam.

virtual void jafar::slam::SlamEkf::predict	(	unsigned int	_robotId,
		jblas::vec const &	u
	)		`[virtual]`

Compute the prediction for the movement of a robot.

Parameters:

_robotId	id of the robot
u	command

Implements jafar::slam::BaseSlam.

Reimplemented in jafar::slam::BearingOnlySlam.

virtual void jafar::slam::SlamEkf::removeFeature ( unsigned int id ) [virtual]

Todo:: Implement with a BaseFeature& in parameter

Implements jafar::slam::BaseSlam.

Reimplemented in jafar::slam::BearingOnlySlam.

BaseRobot* jafar::slam::SlamEkf::robot ( unsigned int _id )

Returns:: the robot corresponding to the id.

const BaseRobot* jafar::slam::SlamEkf::robot ( unsigned int ) const

Returns:: the robot corresponding to the id.

void jafar::slam::SlamEkf::setSensor	(	FeatureObserveModel *	model,
		int	sensorId = `0`
	)

Add a sensor with its feature model.

Enable to deal with multiple sensors.

std::size_t jafar::slam::SlamEkf::sizeMap ( ) const [inline]

Returns:: number of features in the map.

Definition at line 354 of file slamEkf.hpp.

References featuresMap.

virtual void jafar::slam::SlamEkf::writeLogData ( jafar::kernel::DataLogger & log ) const [protected, virtual]

Implements this method calling repeatidly log methods.

You should use writeData() or writeDataVector().

Implements jafar::kernel::DataLoggable.

Reimplemented in jafar::slam::BearingOnlySlam.

virtual void jafar::slam::SlamEkf::writeLogHeader ( jafar::kernel::DataLogger & log ) const [protected, virtual]

Implements this method calling repeatidly log methods.

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

Implements jafar::kernel::DataLoggable.

Reimplemented in jafar::slam::BearingOnlySlam.

void jafar::slam::SlamEkf::writePoseCopyToFile	(	unsigned int	id,
		std::string const &	dir
	)

Write poseCopy id in a t3d file.

write in dir/prefix_id.t3d

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

modules/slam/include/slam/slamEkf.hpp

Detailed Description

Classes

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Types

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Private Member Functions

Friends

Member Enumeration Documentation

Member Function Documentation