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

#include <mrpt/poses/CRobot2DPoseEstimator.h>

Classes
struct	TOptions

Public Member Functions
	CRobot2DPoseEstimator ()
	Default constructor. More...

virtual	~CRobot2DPoseEstimator ()
	Destructor. More...

void	reset ()

void	processUpdateNewPoseLocalization (const mrpt::math::TPose2D &newPose, const mrpt::math::CMatrixDouble33 &newPoseCov, mrpt::system::TTimeStamp cur_tim)
	Updates the filter so the pose is tracked to the current time. More...

void	processUpdateNewOdometry (const mrpt::math::TPose2D &newGlobalOdometry, mrpt::system::TTimeStamp cur_tim, bool hasVelocities=false, float v=0, float w=0)
	Updates the filter so the pose is tracked to the current time. More...

bool	getCurrentEstimate (mrpt::math::TPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
	Get the current estimate, obtained as: More...

bool	getCurrentEstimate (mrpt::poses::CPose2D &pose, float &v, float &w, mrpt::system::TTimeStamp tim_query=mrpt::system::now()) const
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

bool	getLatestRobotPose (mrpt::math::TPose2D &pose) const
	Get the latest known robot pose, either from odometry or localization. More...

bool	getLatestRobotPose (CPose2D &pose) const
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

Public Attributes
TOptions	params
	parameters of the filter. More...

Static Private Member Functions
static void	extrapolateRobotPose (const mrpt::math::TPose2D &p, const float v, const float w, const double delta_time, mrpt::math::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". More...

Private Attributes
mrpt::synch::CCriticalSection	m_cs

mrpt::system::TTimeStamp	m_last_loc_time

mrpt::math::TPose2D	m_last_loc
	Last pose as estimated by the localization/SLAM subsystem. More...

mrpt::math::CMatrixDouble33	m_last_loc_cov

mrpt::math::TPose2D	m_loc_odo_ref
	The interpolated odometry position for the last "m_robot_pose" (used as "coordinates base" for subsequent odo readings) More...

mrpt::system::TTimeStamp	m_last_odo_time

mrpt::math::TPose2D	m_last_odo

float	m_robot_v

float	m_robot_w

Constructor & Destructor Documentation

◆ CRobot2DPoseEstimator()

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

Default constructor.

◆ ~CRobot2DPoseEstimator()

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

virtual

Destructor.

Member Function Documentation

◆ extrapolateRobotPose()

static void mrpt::poses::CRobot2DPoseEstimator::extrapolateRobotPose	(	const mrpt::math::TPose2D &	p,
		const float	v,
		const float	w,
		const double	delta_time,
		mrpt::math::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".

◆ getCurrentEstimate() [1/2]

bool mrpt::poses::CRobot2DPoseEstimator::getCurrentEstimate	(	mrpt::math::TPose2D &	pose,
		float &	v,
		float &	w,
		mrpt::system::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

◆ getCurrentEstimate() [2/2]

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

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

◆ getLatestRobotPose() [1/2]

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

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

◆ getLatestRobotPose() [2/2]

bool mrpt::poses::CRobot2DPoseEstimator::getLatestRobotPose ( mrpt::math::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

◆ processUpdateNewOdometry()

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

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

◆ processUpdateNewPoseLocalization()

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

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

◆ reset()

void mrpt::poses::CRobot2DPoseEstimator::reset ( )

Member Data Documentation

◆ m_cs

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

private

Definition at line 87 of file CRobot2DPoseEstimator.h.

◆ m_last_loc

mrpt::math::TPose2D mrpt::poses::CRobot2DPoseEstimator::m_last_loc

private

Last pose as estimated by the localization/SLAM subsystem.

Definition at line 90 of file CRobot2DPoseEstimator.h.

◆ m_last_loc_cov

mrpt::math::CMatrixDouble33 mrpt::poses::CRobot2DPoseEstimator::m_last_loc_cov

private

Definition at line 91 of file CRobot2DPoseEstimator.h.

◆ m_last_loc_time

mrpt::system::TTimeStamp mrpt::poses::CRobot2DPoseEstimator::m_last_loc_time

private

Definition at line 89 of file CRobot2DPoseEstimator.h.

◆ m_last_odo

mrpt::math::TPose2D mrpt::poses::CRobot2DPoseEstimator::m_last_odo

private

Definition at line 96 of file CRobot2DPoseEstimator.h.

◆ m_last_odo_time

mrpt::system::TTimeStamp mrpt::poses::CRobot2DPoseEstimator::m_last_odo_time

private

Definition at line 95 of file CRobot2DPoseEstimator.h.

◆ m_loc_odo_ref

mrpt::math::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 93 of file CRobot2DPoseEstimator.h.

◆ m_robot_v

float mrpt::poses::CRobot2DPoseEstimator::m_robot_v

private

Definition at line 97 of file CRobot2DPoseEstimator.h.

◆ m_robot_w

float mrpt::poses::CRobot2DPoseEstimator::m_robot_w

private

Definition at line 98 of file CRobot2DPoseEstimator.h.

◆ params

TOptions mrpt::poses::CRobot2DPoseEstimator::params

parameters of the filter.

Definition at line 84 of file CRobot2DPoseEstimator.h.

Detailed Description

Classes

Public Member Functions

Public Attributes

Static Private Member Functions

Private Attributes

Constructor & Destructor Documentation

◆ CRobot2DPoseEstimator()

◆ ~CRobot2DPoseEstimator()

Member Function Documentation

◆ extrapolateRobotPose()

◆ getCurrentEstimate() [1/2]

◆ getCurrentEstimate() [2/2]

◆ getLatestRobotPose() [1/2]

◆ getLatestRobotPose() [2/2]

◆ processUpdateNewOdometry()

◆ processUpdateNewPoseLocalization()

◆ reset()

Member Data Documentation

◆ m_cs

◆ m_last_loc

◆ m_last_loc_cov

◆ m_last_loc_time

◆ m_last_odo

◆ m_last_odo_time

◆ m_loc_odo_ref

◆ m_robot_v

◆ m_robot_w

◆ params