jafar::slam::BearingOnlySlam Class Reference

Bearing only slam. More...

---

Detailed Description

Bearing only slam.

Warning:

only works with one robot

Definition at line 82 of file bearingOnlySlam.hpp.

#include <bearingOnlySlam.hpp>

Inheritance diagram for jafar::slam::BearingOnlySlam:

List of all members.

Public Types
typedef std::map < TrajectoryPoseRecordKey, TrajectoryPoseRecord >	TrajectoryPoseRecordsType
typedef std::map< unsigned int, InitFeature * >	InitFeaturesMapType
Public Member Functions
	BearingOnlySlam (std::size_t sizeMax_, std::size_t sizeRobotState_, std::size_t sizeRobotPose_, std::size_t sizeTrajectory_)
virtual void	init (boost::posix_time::time_duration const &curTime)
	clear the map and the robot pose
virtual void	addRobot (BaseRobot *)
	Add a new BaseRobot in the map.
std::size_t	sizeTrajectory () const
jblas::vec_range const &	getTrajectoryPose (unsigned int frameIndex, unsigned int robotId_=0) const
void	setBoSensor (BearingOnlyFeatureObserveModel *model, int sensorId=0)
	add a bearing-only sensor
void	setBoSensor (BearingOnlyFeatureObserveModel *model, InfFeatureObserveModel *modelInf, int sensorId=0)
	add a bearing-only sensor wich takes into account landmarks at infinity
virtual void	setRobotToSensor (jblas::vec const &robotToSensor, int sensorId=0)
	Set the robotToSensor transformation of sensor sensorId.
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.
bool	hasBoFeatureObserveModel (int sensorId)
BearingOnlyFeatureObserveModel &	getBoFeatureObserveModel (int sensorId=0)
bool	hasInfFeatureObserveModel (int sensorId)
InfFeatureObserveModel &	getInfFeatureObserveModel (int sensorId)
BaseFeatureObserveModel &	getBoInfFeatureObserveModel (int sensorId, bool infinity)
TrajectoryPoseRecordsType const &	getTrajectoryPoseRecords () const
void	addBoEventListener (BoSlamEventListener &listener)
bool	hasInitFeature (unsigned int id_) const
virtual bool	isFeatureKnown (unsigned int id) const
InitFeature &	getInitFeature (unsigned int id) const
virtual void	removeFeature (unsigned int id)
void	removeInitFeature (unsigned int id)
void	removeInitFeature (InitFeaturesMapType::iterator featureIt)
void	beginBrowseInitFeatures ()
bool	hasNextInitFeature ()
InitFeature *	nextInitFeature ()
void	setBoParam (double dMin_=0.5, double dMax_=100, double sprtAlphaFA_=0.01, double sprtAlphaMD_=0.05)
void	predict (unsigned int id, jblas::vec const &u)
	Compute the prediction for the movement of a robot.
Public Attributes
InitFeaturesMapType	initFeatures
InitFeaturesMapType::iterator	browseInitFeaturesIt
Protected Types
typedef std::map< int, BearingOnlyFeatureObserveModel * >	BoFeatureObserveModelsContType
typedef std::map< int, InfFeatureObserveModel * >	InfFeatureObserveModelsContType
typedef std::list < BoSlamEventListener * >	BoSlamEventListenersList
Protected Member Functions
bool	hasTrajectoryPoseRecord (unsigned int frameIndex, unsigned int robotId_=0) const
TrajectoryPoseRecord const &	getTrajectoryPoseRecord (unsigned int frameIndex, unsigned int robotId_=0) const
TrajectoryPoseRecord &	getTrajectoryPoseRecord (unsigned int frameIndex, unsigned int robotId_=0)
void	eraseTrajectoryPoseRecord (unsigned int frameIndex, unsigned int robotId_=0)
void	eraseTrajectoryPoseRecord (TrajectoryPoseRecordsType::iterator it)
void	updateTrajectoryWithRemovedFeature (InitFeature const &f)
void	manageTrajectory (unsigned int robotId_=0)
void	backupCurrentPose (std::size_t index, unsigned int _robotPose=0)
void	observeNewFeature (Observation *obs_)
	This method adds a new feature to the map.
void	observeKnownFeature (Observation *obs_)
	This method performs observation update to a feature in the map.
void	observeInitFeature (InitFeature &feature, Observation *obs)
virtual void	beginProcessObservations (unsigned int robotId_)
virtual void	endProcessObservations (unsigned int robotId_)
void	initState (InitFeature &feature, Observation const &obs)
	init the state of bearing-only feature
void	updateInitState (InitFeature &feature, Observation const &obs)
	update the init state of bearing-only feature
void	cleanInitFeatures ()
	during initialisation steps, unobserved features are deleted
bool	manageInitState (InitFeature &iFeature)
	Manage the Sum Of Gaussians.
void	fullStateInit (InitFeature &iFeature, InitStateMember const &state)
	proceed with all operations to add a feature to the map
void	fullStateInit (InitFeature &iFeature, jblas::vec const &x, jblas::sym_mat const &P, bool atInfinity=false)
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.
Static Protected Member Functions
static double	computeInitStateHypothesisLogLikelihood (InitStateMember &ism, InitFeature &feature, BaseFeatureObserveModel &obsModel, double eraseHypothesisTh, Observation const &obs, jblas::sym_mat const &R, double m2PiN, jblas::vec_range const &xi, jblas::sym_mat_range const &Pi, jblas::vec_range const &xt, jblas::sym_mat_range const &Pt, jblas::sym_mat_range const &Pti, jblas::vec_range &xf_m_r, jblas::sym_mat &S_inv, jblas::mat_range &Jxt, jblas::mat &Jxi, jblas::mat &Jxf)
	compute hypothesis log-likelihood
Protected Attributes
std::vector< jblas::vec_range >	trajectory
std::vector< jblas::sym_mat_range >	trajectoryCov
std::vector< std::size_t >	trajectoryStateIndex
std::vector< bool >	trajectoryIsRobot
std::list< std::size_t >	trajectoryEmptyIndexes
std::map< unsigned int, std::size_t >	robotIdToTrajectoryIndex
TrajectoryPoseRecordsType	trajectoryPoseRecords
BoFeatureObserveModelsContType	boFeatureObserveModels
	Observe models for bearing only features.
InfFeatureObserveModelsContType	infFeatureObserveModels
	Observe models for features at infinity.
double	dMin
	parameter for the depth initialization (min)
double	dMax
	parameter for the depth initialization (max)
double	sprtAlphaFA
	False alarm probability.
double	sprtAlphaMD
	Missed detection probability.
double	logA
	sprt upper bound
double	logB
	sprt lower bound
BoSlamEventListenersList	boSlamEventListeners
unsigned int	nbInitFeaturesLost
	for logging this info
Private Attributes
std::size_t	m_sizeTrajectory
Friends
class	KarmaManager
std::ostream &	operator<< (std::ostream &s, const BearingOnlySlam &slam_)
std::string	boSlamTrajectory (BearingOnlySlam const &slam)

---

Member Function Documentation

virtual void jafar::slam::BearingOnlySlam::addRobot

(

BaseRobot *

)

[virtual]

Add a new BaseRobot in the map.

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

Reimplemented from jafar::slam::SlamEkf.

bool jafar::slam::BearingOnlySlam::manageInitState

(

InitFeature &

iFeature

)

[protected]

Manage the Sum Of Gaussians.

Returns:

true if iFeature still exists.

void jafar::slam::BearingOnlySlam::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 from jafar::slam::SlamEkf.

void jafar::slam::BearingOnlySlam::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

Reimplemented from jafar::slam::SlamEkf.

virtual void jafar::slam::BearingOnlySlam::removeFeature

(

unsigned int

id

)

[virtual]

Todo:

Implement with a BaseFeature& in parameter

Reimplemented from jafar::slam::SlamEkf.

std::size_t jafar::slam::BearingOnlySlam::sizeTrajectory

(

)

const [inline]

Returns:

the number of poses estimated by the slam (current pose + past poses)

Definition at line 228 of file bearingOnlySlam.hpp.

virtual void jafar::slam::BearingOnlySlam::writeLogData

(

jafar::kernel::DataLogger &

log

)

const [protected, virtual]

Implements this method calling repeatidly log methods.

You should use writeData() or writeDataVector().

Reimplemented from jafar::slam::SlamEkf.

virtual void jafar::slam::BearingOnlySlam::writeLogHeader

(

jafar::kernel::DataLogger &

log

)

const [protected, virtual]

Implements this method calling repeatidly log methods.

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

Reimplemented from jafar::slam::SlamEkf.

---

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

• modules/slam/include/slam/bearingOnlySlam.hpp