mrpt::poses::CRobot2DPoseEstimator Class Reference

A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data. More...

#include <mrpt/poses/CRobot2DPoseEstimator.h>

Collaboration diagram for mrpt::poses::CRobot2DPoseEstimator:

[legend]

List of all members.

Classes
struct	TOptions
Public Member Functions
	CRobot2DPoseEstimator ()
	Default constructor.
virtual	~CRobot2DPoseEstimator ()
	Destructor.
void	reset ()
void	processUpdateNewPoseLocalization (const TPose2D &newPose, const CMatrixDouble33 &newPoseCov, TTimeStamp cur_tim)
	Updates the filter so the pose is tracked to the current time.
void	processUpdateNewOdometry (const TPose2D &newGlobalOdometry, TTimeStamp cur_tim, bool hasVelocities=false, float v=0, float w=0)
	Updates the filter so the pose is tracked to the current time.
bool	getCurrentEstimate (TPose2D &pose, float &v, float &w, TTimeStamp tim_query=mrpt::system::now()) const
	Get the current estimate, obtained as:.
bool	getCurrentEstimate (CPose2D &pose, float &v, float &w, TTimeStamp tim_query=mrpt::system::now()) const
	Get the current estimate, obtained as:.
bool	getLatestRobotPose (TPose2D &pose) const
	Get the latest known robot pose, either from odometry or localization.
bool	getLatestRobotPose (CPose2D &pose) const
	Get the latest known robot pose, either from odometry or localization.
Public Attributes
TOptions	params
	parameters of the filter.
Static Private Member Functions
static void	extrapolateRobotPose (const TPose2D &p, const float v, const float w, const double delta_time, TPose2D &new_p)
	An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".
Private Attributes
mrpt::synch::CCriticalSection	m_cs
TTimeStamp	m_last_loc_time
TPose2D	m_last_loc
	Last pose as estimated by the localization/SLAM subsystem.
CMatrixDouble33	m_last_loc_cov
TPose2D	m_loc_odo_ref
	The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings).
TTimeStamp	m_last_odo_time
TPose2D	m_last_odo
float	m_robot_v
float	m_robot_w

Detailed Description

A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry and localization data.

The implemented model is a state vector:

(x,y,phi,v,w) for the robot pose (x,y,phi) and velocities (v,w).

The filter can be asked for an extrapolation for some arbitrary time "t'", and it'll do a simple linear prediction. All methods are thread-safe.

Definition at line 49 of file CRobot2DPoseEstimator.h.

Constructor & Destructor Documentation

mrpt::poses::CRobot2DPoseEstimator::CRobot2DPoseEstimator ( )

Default constructor.

virtual mrpt::poses::CRobot2DPoseEstimator::~CRobot2DPoseEstimator ( ) [virtual]

Destructor.

Member Function Documentation

static void mrpt::poses::CRobot2DPoseEstimator::extrapolateRobotPose	(	const TPose2D &	p,
		const float	v,
		const float	w,
		const double	delta_time,
		TPose2D &	new_p
	)			`[static, private]`

An auxiliary method to extrapolate the pose of a robot located at "p" with velocities (v,w) after a time delay "delta_time".

bool mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate	(	CPose2D &	pose,
		float &	v,
		float &	w,
		TTimeStamp	tim_query = `mrpt::system::now()`
	)			const `[inline]`

Get the current estimate, obtained as:.

last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw

Returns:: true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.

See also:: getLatestRobotPose

Definition at line 86 of file CRobot2DPoseEstimator.h.

bool mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate	(	TPose2D &	pose,
		float &	v,
		float &	w,
		TTimeStamp	tim_query = `mrpt::system::now()`
	)			const

Get the current estimate, obtained as:.

last_loc (+) [ last_odo (-) odo_ref ] (+) extrapolation_from_vw

Returns:: true is the estimate can be trusted. False if the real observed data is too old or there is no valid data yet.

See also:: getLatestRobotPose

bool mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose ( CPose2D & pose ) const [inline]

Get the latest known robot pose, either from odometry or localization.

Returns:: false if there is not estimation yet.

Definition at line 105 of file CRobot2DPoseEstimator.h.

References mrpt::math::TPose2D::phi, mrpt::poses::CPose2D::phi(), mrpt::math::TPose2D::x, mrpt::poses::CPoseOrPoint::x(), mrpt::math::TPose2D::y, and mrpt::poses::CPoseOrPoint::y().

bool mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose ( TPose2D & pose ) const

Get the latest known robot pose, either from odometry or localization.

This differs from getCurrentEstimate() in that this method does NOT extrapolate as getCurrentEstimate() does.

Returns:: false if there is not estimation yet.

See also:: getCurrentEstimate

void mrpt::poses::CRobot2DPoseEstimator::processUpdateNewOdometry	(	const TPose2D &	newGlobalOdometry,
		TTimeStamp	cur_tim,
		bool	hasVelocities = `false`,
		float	v = `0`,
		float	w = `0`
	)

Updates the filter so the pose is tracked to the current time.

void mrpt::poses::CRobot2DPoseEstimator::processUpdateNewPoseLocalization	(	const TPose2D &	newPose,
		const CMatrixDouble33 &	newPoseCov,
		TTimeStamp	cur_tim
	)