mrpt::poses::CRobot2DPoseEstimator Class Reference

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 50 of file CRobot2DPoseEstimator.h.

#include <mrpt/poses/CRobot2DPoseEstimator.h>

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

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  )

staticprivate

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	(	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::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 87 of file CRobot2DPoseEstimator.h.

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

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 106 of file CRobot2DPoseEstimator.h.

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

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
	)

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

void mrpt::poses::CRobot2DPoseEstimator::reset

(

)

Member Data Documentation

mrpt::synch::CCriticalSection mrpt::poses::CRobot2DPoseEstimator::m_cs

private

Definition at line 134 of file CRobot2DPoseEstimator.h.

TPose2D mrpt::poses::CRobot2DPoseEstimator::m_last_loc

private

Last pose as estimated by the localization/SLAM subsystem.

Definition at line 137 of file CRobot2DPoseEstimator.h.

CMatrixDouble33 mrpt::poses::CRobot2DPoseEstimator::m_last_loc_cov

private

Definition at line 138 of file CRobot2DPoseEstimator.h.

TTimeStamp mrpt::poses::CRobot2DPoseEstimator::m_last_loc_time

private

Definition at line 136 of file CRobot2DPoseEstimator.h.

TPose2D mrpt::poses::CRobot2DPoseEstimator::m_last_odo

private

Definition at line 143 of file CRobot2DPoseEstimator.h.

TTimeStamp mrpt::poses::CRobot2DPoseEstimator::m_last_odo_time

private

Definition at line 142 of file CRobot2DPoseEstimator.h.

TPose2D mrpt::poses::CRobot2DPoseEstimator::m_loc_odo_ref

private

The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings)

Definition at line 140 of file CRobot2DPoseEstimator.h.

float mrpt::poses::CRobot2DPoseEstimator::m_robot_v

private

Definition at line 144 of file CRobot2DPoseEstimator.h.

float mrpt::poses::CRobot2DPoseEstimator::m_robot_w

private

Definition at line 145 of file CRobot2DPoseEstimator.h.

TOptions mrpt::poses::CRobot2DPoseEstimator::params

parameters of the filter.

Definition at line 131 of file CRobot2DPoseEstimator.h.